View Javadoc
1   package com.irurueta.navigation.inertial.calibration.accelerometer;
2   
3   import com.irurueta.algebra.Matrix;
4   import com.irurueta.algebra.WrongSizeException;
5   import com.irurueta.navigation.LockedException;
6   import com.irurueta.navigation.NotReadyException;
7   import com.irurueta.navigation.frames.CoordinateTransformation;
8   import com.irurueta.navigation.frames.ECEFFrame;
9   import com.irurueta.navigation.frames.FrameType;
10  import com.irurueta.navigation.frames.InvalidSourceAndDestinationFrameTypeException;
11  import com.irurueta.navigation.frames.NEDFrame;
12  import com.irurueta.navigation.frames.converters.NEDtoECEFFrameConverter;
13  import com.irurueta.navigation.frames.converters.NEDtoECEFPositionVelocityConverter;
14  import com.irurueta.navigation.inertial.BodyKinematics;
15  import com.irurueta.navigation.inertial.ECEFGravity;
16  import com.irurueta.navigation.inertial.ECEFPosition;
17  import com.irurueta.navigation.inertial.ECEFVelocity;
18  import com.irurueta.navigation.inertial.NEDPosition;
19  import com.irurueta.navigation.inertial.NEDVelocity;
20  import com.irurueta.navigation.inertial.calibration.BodyKinematicsGenerator;
21  import com.irurueta.navigation.inertial.calibration.CalibrationException;
22  import com.irurueta.navigation.inertial.calibration.IMUErrors;
23  import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyKinematics;
24  import com.irurueta.navigation.inertial.estimators.ECEFGravityEstimator;
25  import com.irurueta.navigation.inertial.estimators.ECEFKinematicsEstimator;
26  import com.irurueta.statistics.UniformRandomizer;
27  import com.irurueta.units.Acceleration;
28  import com.irurueta.units.AccelerationUnit;
29  import org.junit.Test;
30  
31  import java.util.ArrayList;
32  import java.util.Collection;
33  import java.util.Collections;
34  import java.util.List;
35  import java.util.Random;
36  
37  import static org.junit.Assert.*;
38  
39  public class KnownGravityNormAccelerometerCalibratorTest implements
40          KnownGravityNormAccelerometerCalibratorListener {
41  
42      private static final double TIME_INTERVAL_SECONDS = 0.02;
43  
44      private static final double MICRO_G_TO_METERS_PER_SECOND_SQUARED = 9.80665E-6;
45      private static final double DEG_TO_RAD = 0.01745329252;
46  
47      private static final double MIN_ANGLE_DEGREES = -180.0;
48      private static final double MAX_ANGLE_DEGREES = 180.0;
49  
50      private static final double MIN_LATITUDE_DEGREES = -90.0;
51      private static final double MAX_LATITUDE_DEGREES = 90.0;
52      private static final double MIN_LONGITUDE_DEGREES = -180.0;
53      private static final double MAX_LONGITUDE_DEGREES = 180.0;
54      private static final double MIN_HEIGHT = -50.0;
55      private static final double MAX_HEIGHT = 50.0;
56  
57      private static final int LARGE_MEASUREMENT_NUMBER = 100000;
58  
59      private static final double ABSOLUTE_ERROR = 1e-8;
60      private static final double LARGE_ABSOLUTE_ERROR = 5e-5;
61      private static final double VERY_LARGE_ABSOLUTE_ERROR = 1e-3;
62  
63      private static final int TIMES = 100;
64  
65      private int mCalibrateStart;
66      private int mCalibrateEnd;
67  
68      @Test
69      public void testConstructor1() throws WrongSizeException {
70          final KnownGravityNormAccelerometerCalibrator calibrator =
71                  new KnownGravityNormAccelerometerCalibrator();
72  
73          // check default values
74          assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
75          assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
76          assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
77          final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
78          assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
79          assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
80          final Acceleration bx2 = new Acceleration(0.0,
81                  AccelerationUnit.FEET_PER_SQUARED_SECOND);
82          calibrator.getInitialBiasXAsAcceleration(bx2);
83          assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
84          assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
85          final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
86          assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
87          assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
88          final Acceleration by2 = new Acceleration(0.0,
89                  AccelerationUnit.FEET_PER_SQUARED_SECOND);
90          calibrator.getInitialBiasYAsAcceleration(by2);
91          assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
92          assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
93          final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
94          assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
95          assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
96          final Acceleration bz2 = new Acceleration(0.0,
97                  AccelerationUnit.FEET_PER_SQUARED_SECOND);
98          calibrator.getInitialBiasZAsAcceleration(bz2);
99          assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
100         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
101         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
102         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
103         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
104         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
105         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
106         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
107         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
108         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
109         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
110         final double[] bias1 = calibrator.getInitialBias();
111         assertArrayEquals(bias1, new double[3], 0.0);
112         final double[] bias2 = new double[3];
113         calibrator.getInitialBias(bias2);
114         assertArrayEquals(bias1, bias2, 0.0);
115         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
116         assertEquals(b1, new Matrix(3, 1));
117         final Matrix b2 = new Matrix(3, 1);
118         calibrator.getInitialBiasAsMatrix(b2);
119         assertEquals(b1, b2);
120         final Matrix ma1 = calibrator.getInitialMa();
121         assertEquals(ma1, new Matrix(3, 3));
122         final Matrix ma2 = new Matrix(3, 3);
123         calibrator.getInitialMa(ma2);
124         assertEquals(ma1, ma2);
125         assertNull(calibrator.getMeasurements());
126         assertFalse(calibrator.isCommonAxisUsed());
127         assertNull(calibrator.getListener());
128         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
129         assertFalse(calibrator.isReady());
130         assertFalse(calibrator.isRunning());
131         assertNull(calibrator.getEstimatedBiases());
132         assertFalse(calibrator.getEstimatedBiases(null));
133         assertNull(calibrator.getEstimatedBiasesAsMatrix());
134         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
135         assertNull(calibrator.getEstimatedBiasFx());
136         assertNull(calibrator.getEstimatedBiasFy());
137         assertNull(calibrator.getEstimatedBiasFz());
138         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
139         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
140         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
141         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
142         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
143         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
144         assertNull(calibrator.getEstimatedMa());
145         assertNull(calibrator.getEstimatedSx());
146         assertNull(calibrator.getEstimatedSy());
147         assertNull(calibrator.getEstimatedSz());
148         assertNull(calibrator.getEstimatedMxy());
149         assertNull(calibrator.getEstimatedMxz());
150         assertNull(calibrator.getEstimatedMyx());
151         assertNull(calibrator.getEstimatedMyz());
152         assertNull(calibrator.getEstimatedMzx());
153         assertNull(calibrator.getEstimatedMzy());
154         assertNull(calibrator.getEstimatedCovariance());
155         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
156         assertNull(calibrator.getGroundTruthGravityNorm());
157         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
158         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
159     }
160 
161     @Test
162     public void testConstructor2() throws WrongSizeException {
163         final KnownGravityNormAccelerometerCalibrator calibrator =
164                 new KnownGravityNormAccelerometerCalibrator(this);
165 
166         // check default values
167         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
168         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
169         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
170         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
171         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
172         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
173         final Acceleration bx2 = new Acceleration(0.0,
174                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
175         calibrator.getInitialBiasXAsAcceleration(bx2);
176         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
177         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
178         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
179         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
180         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
181         final Acceleration by2 = new Acceleration(0.0,
182                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
183         calibrator.getInitialBiasYAsAcceleration(by2);
184         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
185         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
186         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
187         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
188         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
189         final Acceleration bz2 = new Acceleration(0.0,
190                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
191         calibrator.getInitialBiasZAsAcceleration(bz2);
192         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
193         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
194         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
195         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
196         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
197         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
198         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
199         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
200         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
201         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
202         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
203         final double[] bias1 = calibrator.getInitialBias();
204         assertArrayEquals(bias1, new double[3], 0.0);
205         final double[] bias2 = new double[3];
206         calibrator.getInitialBias(bias2);
207         assertArrayEquals(bias1, bias2, 0.0);
208         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
209         assertEquals(b1, new Matrix(3, 1));
210         final Matrix b2 = new Matrix(3, 1);
211         calibrator.getInitialBiasAsMatrix(b2);
212         assertEquals(b1, b2);
213         final Matrix ma1 = calibrator.getInitialMa();
214         assertEquals(ma1, new Matrix(3, 3));
215         final Matrix ma2 = new Matrix(3, 3);
216         calibrator.getInitialMa(ma2);
217         assertEquals(ma1, ma2);
218         assertNull(calibrator.getMeasurements());
219         assertFalse(calibrator.isCommonAxisUsed());
220         assertSame(calibrator.getListener(), this);
221         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
222         assertFalse(calibrator.isReady());
223         assertFalse(calibrator.isRunning());
224         assertNull(calibrator.getEstimatedBiases());
225         assertFalse(calibrator.getEstimatedBiases(null));
226         assertNull(calibrator.getEstimatedBiasesAsMatrix());
227         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
228         assertNull(calibrator.getEstimatedBiasFx());
229         assertNull(calibrator.getEstimatedBiasFy());
230         assertNull(calibrator.getEstimatedBiasFz());
231         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
232         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
233         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
234         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
235         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
236         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
237         assertNull(calibrator.getEstimatedMa());
238         assertNull(calibrator.getEstimatedSx());
239         assertNull(calibrator.getEstimatedSy());
240         assertNull(calibrator.getEstimatedSz());
241         assertNull(calibrator.getEstimatedMxy());
242         assertNull(calibrator.getEstimatedMxz());
243         assertNull(calibrator.getEstimatedMyx());
244         assertNull(calibrator.getEstimatedMyz());
245         assertNull(calibrator.getEstimatedMzx());
246         assertNull(calibrator.getEstimatedMzy());
247         assertNull(calibrator.getEstimatedCovariance());
248         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
249         assertNull(calibrator.getGroundTruthGravityNorm());
250         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
251         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
252     }
253 
254     @Test
255     public void testConstructor3() throws WrongSizeException {
256         final Collection<StandardDeviationBodyKinematics> measurements =
257                 Collections.emptyList();
258         final KnownGravityNormAccelerometerCalibrator calibrator =
259                 new KnownGravityNormAccelerometerCalibrator(measurements);
260 
261         // check default values
262         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
263         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
264         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
265         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
266         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
267         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
268         final Acceleration bx2 = new Acceleration(0.0,
269                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
270         calibrator.getInitialBiasXAsAcceleration(bx2);
271         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
272         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
273         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
274         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
275         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
276         final Acceleration by2 = new Acceleration(0.0,
277                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
278         calibrator.getInitialBiasYAsAcceleration(by2);
279         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
280         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
281         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
282         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
283         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
284         final Acceleration bz2 = new Acceleration(0.0,
285                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
286         calibrator.getInitialBiasZAsAcceleration(bz2);
287         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
288         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
289         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
290         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
291         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
292         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
293         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
294         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
295         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
296         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
297         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
298         final double[] bias1 = calibrator.getInitialBias();
299         assertArrayEquals(bias1, new double[3], 0.0);
300         final double[] bias2 = new double[3];
301         calibrator.getInitialBias(bias2);
302         assertArrayEquals(bias1, bias2, 0.0);
303         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
304         assertEquals(b1, new Matrix(3, 1));
305         final Matrix b2 = new Matrix(3, 1);
306         calibrator.getInitialBiasAsMatrix(b2);
307         assertEquals(b1, b2);
308         final Matrix ma1 = calibrator.getInitialMa();
309         assertEquals(ma1, new Matrix(3, 3));
310         final Matrix ma2 = new Matrix(3, 3);
311         calibrator.getInitialMa(ma2);
312         assertEquals(ma1, ma2);
313         assertSame(calibrator.getMeasurements(), measurements);
314         assertFalse(calibrator.isCommonAxisUsed());
315         assertNull(calibrator.getListener());
316         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
317         assertFalse(calibrator.isReady());
318         assertFalse(calibrator.isRunning());
319         assertNull(calibrator.getEstimatedBiases());
320         assertFalse(calibrator.getEstimatedBiases(null));
321         assertNull(calibrator.getEstimatedBiasesAsMatrix());
322         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
323         assertNull(calibrator.getEstimatedBiasFx());
324         assertNull(calibrator.getEstimatedBiasFy());
325         assertNull(calibrator.getEstimatedBiasFz());
326         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
327         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
328         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
329         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
330         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
331         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
332         assertNull(calibrator.getEstimatedMa());
333         assertNull(calibrator.getEstimatedSx());
334         assertNull(calibrator.getEstimatedSy());
335         assertNull(calibrator.getEstimatedSz());
336         assertNull(calibrator.getEstimatedMxy());
337         assertNull(calibrator.getEstimatedMxz());
338         assertNull(calibrator.getEstimatedMyx());
339         assertNull(calibrator.getEstimatedMyz());
340         assertNull(calibrator.getEstimatedMzx());
341         assertNull(calibrator.getEstimatedMzy());
342         assertNull(calibrator.getEstimatedCovariance());
343         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
344         assertNull(calibrator.getGroundTruthGravityNorm());
345         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
346         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
347     }
348 
349     @Test
350     public void testConstructor4() throws WrongSizeException {
351         final Collection<StandardDeviationBodyKinematics> measurements =
352                 Collections.emptyList();
353         final KnownGravityNormAccelerometerCalibrator calibrator =
354                 new KnownGravityNormAccelerometerCalibrator(measurements,
355                         this);
356 
357         // check default values
358         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
359         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
360         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
361         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
362         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
363         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
364         final Acceleration bx2 = new Acceleration(0.0,
365                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
366         calibrator.getInitialBiasXAsAcceleration(bx2);
367         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
368         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
369         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
370         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
371         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
372         final Acceleration by2 = new Acceleration(0.0,
373                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
374         calibrator.getInitialBiasYAsAcceleration(by2);
375         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
376         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
377         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
378         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
379         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
380         final Acceleration bz2 = new Acceleration(0.0,
381                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
382         calibrator.getInitialBiasZAsAcceleration(bz2);
383         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
384         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
385         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
386         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
387         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
388         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
389         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
390         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
391         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
392         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
393         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
394         final double[] bias1 = calibrator.getInitialBias();
395         assertArrayEquals(bias1, new double[3], 0.0);
396         final double[] bias2 = new double[3];
397         calibrator.getInitialBias(bias2);
398         assertArrayEquals(bias1, bias2, 0.0);
399         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
400         assertEquals(b1, new Matrix(3, 1));
401         final Matrix b2 = new Matrix(3, 1);
402         calibrator.getInitialBiasAsMatrix(b2);
403         assertEquals(b1, b2);
404         final Matrix ma1 = calibrator.getInitialMa();
405         assertEquals(ma1, new Matrix(3, 3));
406         final Matrix ma2 = new Matrix(3, 3);
407         calibrator.getInitialMa(ma2);
408         assertEquals(ma1, ma2);
409         assertSame(calibrator.getMeasurements(), measurements);
410         assertFalse(calibrator.isCommonAxisUsed());
411         assertSame(calibrator.getListener(), this);
412         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
413         assertFalse(calibrator.isReady());
414         assertFalse(calibrator.isRunning());
415         assertNull(calibrator.getEstimatedBiases());
416         assertFalse(calibrator.getEstimatedBiases(null));
417         assertNull(calibrator.getEstimatedBiasesAsMatrix());
418         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
419         assertNull(calibrator.getEstimatedBiasFx());
420         assertNull(calibrator.getEstimatedBiasFy());
421         assertNull(calibrator.getEstimatedBiasFz());
422         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
423         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
424         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
425         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
426         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
427         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
428         assertNull(calibrator.getEstimatedMa());
429         assertNull(calibrator.getEstimatedSx());
430         assertNull(calibrator.getEstimatedSy());
431         assertNull(calibrator.getEstimatedSz());
432         assertNull(calibrator.getEstimatedMxy());
433         assertNull(calibrator.getEstimatedMxz());
434         assertNull(calibrator.getEstimatedMyx());
435         assertNull(calibrator.getEstimatedMyz());
436         assertNull(calibrator.getEstimatedMzx());
437         assertNull(calibrator.getEstimatedMzy());
438         assertNull(calibrator.getEstimatedCovariance());
439         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
440         assertNull(calibrator.getGroundTruthGravityNorm());
441         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
442         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
443     }
444 
445     @Test
446     public void testConstructor5() throws WrongSizeException {
447         final KnownGravityNormAccelerometerCalibrator calibrator =
448                 new KnownGravityNormAccelerometerCalibrator(true);
449 
450         // check default values
451         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
452         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
453         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
454         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
455         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
456         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
457         final Acceleration bx2 = new Acceleration(0.0,
458                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
459         calibrator.getInitialBiasXAsAcceleration(bx2);
460         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
461         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
462         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
463         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
464         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
465         final Acceleration by2 = new Acceleration(0.0,
466                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
467         calibrator.getInitialBiasYAsAcceleration(by2);
468         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
469         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
470         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
471         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
472         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
473         final Acceleration bz2 = new Acceleration(0.0,
474                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
475         calibrator.getInitialBiasZAsAcceleration(bz2);
476         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
477         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
478         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
479         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
480         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
481         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
482         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
483         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
484         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
485         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
486         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
487         final double[] bias1 = calibrator.getInitialBias();
488         assertArrayEquals(bias1, new double[3], 0.0);
489         final double[] bias2 = new double[3];
490         calibrator.getInitialBias(bias2);
491         assertArrayEquals(bias1, bias2, 0.0);
492         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
493         assertEquals(b1, new Matrix(3, 1));
494         final Matrix b2 = new Matrix(3, 1);
495         calibrator.getInitialBiasAsMatrix(b2);
496         assertEquals(b1, b2);
497         final Matrix ma1 = calibrator.getInitialMa();
498         assertEquals(ma1, new Matrix(3, 3));
499         final Matrix ma2 = new Matrix(3, 3);
500         calibrator.getInitialMa(ma2);
501         assertEquals(ma1, ma2);
502         assertNull(calibrator.getMeasurements());
503         assertTrue(calibrator.isCommonAxisUsed());
504         assertNull(calibrator.getListener());
505         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
506         assertFalse(calibrator.isReady());
507         assertFalse(calibrator.isRunning());
508         assertNull(calibrator.getEstimatedBiases());
509         assertFalse(calibrator.getEstimatedBiases(null));
510         assertNull(calibrator.getEstimatedBiasesAsMatrix());
511         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
512         assertNull(calibrator.getEstimatedBiasFx());
513         assertNull(calibrator.getEstimatedBiasFy());
514         assertNull(calibrator.getEstimatedBiasFz());
515         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
516         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
517         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
518         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
519         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
520         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
521         assertNull(calibrator.getEstimatedMa());
522         assertNull(calibrator.getEstimatedSx());
523         assertNull(calibrator.getEstimatedSy());
524         assertNull(calibrator.getEstimatedSz());
525         assertNull(calibrator.getEstimatedMxy());
526         assertNull(calibrator.getEstimatedMxz());
527         assertNull(calibrator.getEstimatedMyx());
528         assertNull(calibrator.getEstimatedMyz());
529         assertNull(calibrator.getEstimatedMzx());
530         assertNull(calibrator.getEstimatedMzy());
531         assertNull(calibrator.getEstimatedCovariance());
532         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
533         assertNull(calibrator.getGroundTruthGravityNorm());
534         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
535         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
536     }
537 
538     @Test
539     public void testConstructor6() throws WrongSizeException {
540         final KnownGravityNormAccelerometerCalibrator calibrator =
541                 new KnownGravityNormAccelerometerCalibrator(
542                         true, this);
543 
544         // check default values
545         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
546         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
547         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
548         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
549         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
550         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
551         final Acceleration bx2 = new Acceleration(0.0,
552                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
553         calibrator.getInitialBiasXAsAcceleration(bx2);
554         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
555         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
556         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
557         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
558         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
559         final Acceleration by2 = new Acceleration(0.0,
560                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
561         calibrator.getInitialBiasYAsAcceleration(by2);
562         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
563         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
564         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
565         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
566         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
567         final Acceleration bz2 = new Acceleration(0.0,
568                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
569         calibrator.getInitialBiasZAsAcceleration(bz2);
570         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
571         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
572         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
573         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
574         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
575         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
576         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
577         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
578         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
579         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
580         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
581         final double[] bias1 = calibrator.getInitialBias();
582         assertArrayEquals(bias1, new double[3], 0.0);
583         final double[] bias2 = new double[3];
584         calibrator.getInitialBias(bias2);
585         assertArrayEquals(bias1, bias2, 0.0);
586         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
587         assertEquals(b1, new Matrix(3, 1));
588         final Matrix b2 = new Matrix(3, 1);
589         calibrator.getInitialBiasAsMatrix(b2);
590         assertEquals(b1, b2);
591         final Matrix ma1 = calibrator.getInitialMa();
592         assertEquals(ma1, new Matrix(3, 3));
593         final Matrix ma2 = new Matrix(3, 3);
594         calibrator.getInitialMa(ma2);
595         assertEquals(ma1, ma2);
596         assertNull(calibrator.getMeasurements());
597         assertTrue(calibrator.isCommonAxisUsed());
598         assertSame(calibrator.getListener(), this);
599         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
600         assertFalse(calibrator.isReady());
601         assertFalse(calibrator.isRunning());
602         assertNull(calibrator.getEstimatedBiases());
603         assertFalse(calibrator.getEstimatedBiases(null));
604         assertNull(calibrator.getEstimatedBiasesAsMatrix());
605         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
606         assertNull(calibrator.getEstimatedBiasFx());
607         assertNull(calibrator.getEstimatedBiasFy());
608         assertNull(calibrator.getEstimatedBiasFz());
609         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
610         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
611         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
612         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
613         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
614         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
615         assertNull(calibrator.getEstimatedMa());
616         assertNull(calibrator.getEstimatedSx());
617         assertNull(calibrator.getEstimatedSy());
618         assertNull(calibrator.getEstimatedSz());
619         assertNull(calibrator.getEstimatedMxy());
620         assertNull(calibrator.getEstimatedMxz());
621         assertNull(calibrator.getEstimatedMyx());
622         assertNull(calibrator.getEstimatedMyz());
623         assertNull(calibrator.getEstimatedMzx());
624         assertNull(calibrator.getEstimatedMzy());
625         assertNull(calibrator.getEstimatedCovariance());
626         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
627         assertNull(calibrator.getGroundTruthGravityNorm());
628         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
629         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
630     }
631 
632     @Test
633     public void testConstructor7() throws WrongSizeException {
634         final Collection<StandardDeviationBodyKinematics> measurements =
635                 Collections.emptyList();
636         final KnownGravityNormAccelerometerCalibrator calibrator =
637                 new KnownGravityNormAccelerometerCalibrator(measurements,
638                         true);
639 
640         // check default values
641         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
642         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
643         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
644         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
645         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
646         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
647         final Acceleration bx2 = new Acceleration(0.0,
648                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
649         calibrator.getInitialBiasXAsAcceleration(bx2);
650         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
651         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
652         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
653         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
654         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
655         final Acceleration by2 = new Acceleration(0.0,
656                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
657         calibrator.getInitialBiasYAsAcceleration(by2);
658         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
659         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
660         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
661         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
662         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
663         final Acceleration bz2 = new Acceleration(0.0,
664                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
665         calibrator.getInitialBiasZAsAcceleration(bz2);
666         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
667         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
668         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
669         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
670         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
671         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
672         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
673         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
674         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
675         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
676         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
677         final double[] bias1 = calibrator.getInitialBias();
678         assertArrayEquals(bias1, new double[3], 0.0);
679         final double[] bias2 = new double[3];
680         calibrator.getInitialBias(bias2);
681         assertArrayEquals(bias1, bias2, 0.0);
682         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
683         assertEquals(b1, new Matrix(3, 1));
684         final Matrix b2 = new Matrix(3, 1);
685         calibrator.getInitialBiasAsMatrix(b2);
686         assertEquals(b1, b2);
687         final Matrix ma1 = calibrator.getInitialMa();
688         assertEquals(ma1, new Matrix(3, 3));
689         final Matrix ma2 = new Matrix(3, 3);
690         calibrator.getInitialMa(ma2);
691         assertEquals(ma1, ma2);
692         assertSame(calibrator.getMeasurements(), measurements);
693         assertTrue(calibrator.isCommonAxisUsed());
694         assertNull(calibrator.getListener());
695         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
696         assertFalse(calibrator.isReady());
697         assertFalse(calibrator.isRunning());
698         assertNull(calibrator.getEstimatedBiases());
699         assertFalse(calibrator.getEstimatedBiases(null));
700         assertNull(calibrator.getEstimatedBiasesAsMatrix());
701         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
702         assertNull(calibrator.getEstimatedBiasFx());
703         assertNull(calibrator.getEstimatedBiasFy());
704         assertNull(calibrator.getEstimatedBiasFz());
705         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
706         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
707         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
708         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
709         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
710         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
711         assertNull(calibrator.getEstimatedMa());
712         assertNull(calibrator.getEstimatedSx());
713         assertNull(calibrator.getEstimatedSy());
714         assertNull(calibrator.getEstimatedSz());
715         assertNull(calibrator.getEstimatedMxy());
716         assertNull(calibrator.getEstimatedMxz());
717         assertNull(calibrator.getEstimatedMyx());
718         assertNull(calibrator.getEstimatedMyz());
719         assertNull(calibrator.getEstimatedMzx());
720         assertNull(calibrator.getEstimatedMzy());
721         assertNull(calibrator.getEstimatedCovariance());
722         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
723         assertNull(calibrator.getGroundTruthGravityNorm());
724         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
725         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
726     }
727 
728     @Test
729     public void testConstructor8() throws WrongSizeException {
730         final Collection<StandardDeviationBodyKinematics> measurements =
731                 Collections.emptyList();
732         final KnownGravityNormAccelerometerCalibrator calibrator =
733                 new KnownGravityNormAccelerometerCalibrator(measurements,
734                         true, this);
735 
736         // check default values
737         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
738         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
739         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
740         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
741         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
742         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
743         final Acceleration bx2 = new Acceleration(0.0,
744                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
745         calibrator.getInitialBiasXAsAcceleration(bx2);
746         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
747         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
748         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
749         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
750         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
751         final Acceleration by2 = new Acceleration(0.0,
752                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
753         calibrator.getInitialBiasYAsAcceleration(by2);
754         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
755         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
756         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
757         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
758         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
759         final Acceleration bz2 = new Acceleration(0.0,
760                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
761         calibrator.getInitialBiasZAsAcceleration(bz2);
762         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
763         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
764         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
765         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
766         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
767         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
768         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
769         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
770         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
771         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
772         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
773         final double[] bias1 = calibrator.getInitialBias();
774         assertArrayEquals(bias1, new double[3], 0.0);
775         final double[] bias2 = new double[3];
776         calibrator.getInitialBias(bias2);
777         assertArrayEquals(bias1, bias2, 0.0);
778         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
779         assertEquals(b1, new Matrix(3, 1));
780         final Matrix b2 = new Matrix(3, 1);
781         calibrator.getInitialBiasAsMatrix(b2);
782         assertEquals(b1, b2);
783         final Matrix ma1 = calibrator.getInitialMa();
784         assertEquals(ma1, new Matrix(3, 3));
785         final Matrix ma2 = new Matrix(3, 3);
786         calibrator.getInitialMa(ma2);
787         assertEquals(ma1, ma2);
788         assertSame(calibrator.getMeasurements(), measurements);
789         assertTrue(calibrator.isCommonAxisUsed());
790         assertSame(calibrator.getListener(), this);
791         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
792         assertFalse(calibrator.isReady());
793         assertFalse(calibrator.isRunning());
794         assertNull(calibrator.getEstimatedBiases());
795         assertFalse(calibrator.getEstimatedBiases(null));
796         assertNull(calibrator.getEstimatedBiasesAsMatrix());
797         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
798         assertNull(calibrator.getEstimatedBiasFx());
799         assertNull(calibrator.getEstimatedBiasFy());
800         assertNull(calibrator.getEstimatedBiasFz());
801         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
802         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
803         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
804         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
805         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
806         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
807         assertNull(calibrator.getEstimatedMa());
808         assertNull(calibrator.getEstimatedSx());
809         assertNull(calibrator.getEstimatedSy());
810         assertNull(calibrator.getEstimatedSz());
811         assertNull(calibrator.getEstimatedMxy());
812         assertNull(calibrator.getEstimatedMxz());
813         assertNull(calibrator.getEstimatedMyx());
814         assertNull(calibrator.getEstimatedMyz());
815         assertNull(calibrator.getEstimatedMzx());
816         assertNull(calibrator.getEstimatedMzy());
817         assertNull(calibrator.getEstimatedCovariance());
818         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
819         assertNull(calibrator.getGroundTruthGravityNorm());
820         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
821         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
822     }
823 
824     @Test
825     public void testConstructor9() throws WrongSizeException {
826         final Matrix ba = generateBa();
827         final double biasX = ba.getElementAtIndex(0);
828         final double biasY = ba.getElementAtIndex(1);
829         final double biasZ = ba.getElementAtIndex(2);
830 
831         final KnownGravityNormAccelerometerCalibrator calibrator =
832                 new KnownGravityNormAccelerometerCalibrator(biasX, biasY, biasZ);
833 
834         // check default values
835         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
836         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
837         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
838         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
839         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
840         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
841         final Acceleration bx2 = new Acceleration(0.0,
842                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
843         calibrator.getInitialBiasXAsAcceleration(bx2);
844         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
845         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
846         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
847         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
848         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
849         final Acceleration by2 = new Acceleration(0.0,
850                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
851         calibrator.getInitialBiasYAsAcceleration(by2);
852         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
853         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
854         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
855         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
856         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
857         final Acceleration bz2 = new Acceleration(0.0,
858                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
859         calibrator.getInitialBiasZAsAcceleration(bz2);
860         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
861         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
862         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
863         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
864         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
865         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
866         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
867         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
868         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
869         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
870         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
871         final double[] bias1 = calibrator.getInitialBias();
872         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
873         final double[] bias2 = new double[3];
874         calibrator.getInitialBias(bias2);
875         assertArrayEquals(bias1, bias2, 0.0);
876         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
877         assertEquals(b1, ba);
878         final Matrix b2 = new Matrix(3, 1);
879         calibrator.getInitialBiasAsMatrix(b2);
880         assertEquals(b1, b2);
881         final Matrix ma1 = calibrator.getInitialMa();
882         assertEquals(ma1, new Matrix(3, 3));
883         final Matrix ma2 = new Matrix(3, 3);
884         calibrator.getInitialMa(ma2);
885         assertEquals(ma1, ma2);
886         assertNull(calibrator.getMeasurements());
887         assertFalse(calibrator.isCommonAxisUsed());
888         assertNull(calibrator.getListener());
889         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
890         assertFalse(calibrator.isReady());
891         assertFalse(calibrator.isRunning());
892         assertNull(calibrator.getEstimatedBiases());
893         assertFalse(calibrator.getEstimatedBiases(null));
894         assertNull(calibrator.getEstimatedBiasesAsMatrix());
895         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
896         assertNull(calibrator.getEstimatedBiasFx());
897         assertNull(calibrator.getEstimatedBiasFy());
898         assertNull(calibrator.getEstimatedBiasFz());
899         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
900         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
901         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
902         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
903         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
904         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
905         assertNull(calibrator.getEstimatedMa());
906         assertNull(calibrator.getEstimatedSx());
907         assertNull(calibrator.getEstimatedSy());
908         assertNull(calibrator.getEstimatedSz());
909         assertNull(calibrator.getEstimatedMxy());
910         assertNull(calibrator.getEstimatedMxz());
911         assertNull(calibrator.getEstimatedMyx());
912         assertNull(calibrator.getEstimatedMyz());
913         assertNull(calibrator.getEstimatedMzx());
914         assertNull(calibrator.getEstimatedMzy());
915         assertNull(calibrator.getEstimatedCovariance());
916         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
917         assertNull(calibrator.getGroundTruthGravityNorm());
918         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
919         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
920     }
921 
922     @Test
923     public void testConstructor10() throws WrongSizeException {
924         final Matrix ba = generateBa();
925         final double biasX = ba.getElementAtIndex(0);
926         final double biasY = ba.getElementAtIndex(1);
927         final double biasZ = ba.getElementAtIndex(2);
928 
929         final KnownGravityNormAccelerometerCalibrator calibrator =
930                 new KnownGravityNormAccelerometerCalibrator(
931                         biasX, biasY, biasZ, this);
932 
933         // check default values
934         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
935         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
936         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
937         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
938         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
939         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
940         final Acceleration bx2 = new Acceleration(0.0,
941                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
942         calibrator.getInitialBiasXAsAcceleration(bx2);
943         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
944         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
945         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
946         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
947         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
948         final Acceleration by2 = new Acceleration(0.0,
949                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
950         calibrator.getInitialBiasYAsAcceleration(by2);
951         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
952         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
953         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
954         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
955         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
956         final Acceleration bz2 = new Acceleration(0.0,
957                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
958         calibrator.getInitialBiasZAsAcceleration(bz2);
959         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
960         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
961         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
962         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
963         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
964         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
965         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
966         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
967         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
968         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
969         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
970         final double[] bias1 = calibrator.getInitialBias();
971         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
972         final double[] bias2 = new double[3];
973         calibrator.getInitialBias(bias2);
974         assertArrayEquals(bias1, bias2, 0.0);
975         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
976         assertEquals(b1, ba);
977         final Matrix b2 = new Matrix(3, 1);
978         calibrator.getInitialBiasAsMatrix(b2);
979         assertEquals(b1, b2);
980         final Matrix ma1 = calibrator.getInitialMa();
981         assertEquals(ma1, new Matrix(3, 3));
982         final Matrix ma2 = new Matrix(3, 3);
983         calibrator.getInitialMa(ma2);
984         assertEquals(ma1, ma2);
985         assertNull(calibrator.getMeasurements());
986         assertFalse(calibrator.isCommonAxisUsed());
987         assertSame(calibrator.getListener(), this);
988         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
989         assertFalse(calibrator.isReady());
990         assertFalse(calibrator.isRunning());
991         assertNull(calibrator.getEstimatedBiases());
992         assertFalse(calibrator.getEstimatedBiases(null));
993         assertNull(calibrator.getEstimatedBiasesAsMatrix());
994         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
995         assertNull(calibrator.getEstimatedBiasFx());
996         assertNull(calibrator.getEstimatedBiasFy());
997         assertNull(calibrator.getEstimatedBiasFz());
998         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
999         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1000         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1001         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1002         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1003         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1004         assertNull(calibrator.getEstimatedMa());
1005         assertNull(calibrator.getEstimatedSx());
1006         assertNull(calibrator.getEstimatedSy());
1007         assertNull(calibrator.getEstimatedSz());
1008         assertNull(calibrator.getEstimatedMxy());
1009         assertNull(calibrator.getEstimatedMxz());
1010         assertNull(calibrator.getEstimatedMyx());
1011         assertNull(calibrator.getEstimatedMyz());
1012         assertNull(calibrator.getEstimatedMzx());
1013         assertNull(calibrator.getEstimatedMzy());
1014         assertNull(calibrator.getEstimatedCovariance());
1015         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1016         assertNull(calibrator.getGroundTruthGravityNorm());
1017         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1018         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1019     }
1020 
1021     @Test
1022     public void testConstructor11() throws WrongSizeException {
1023         final Collection<StandardDeviationBodyKinematics> measurements =
1024                 Collections.emptyList();
1025 
1026         final Matrix ba = generateBa();
1027         final double biasX = ba.getElementAtIndex(0);
1028         final double biasY = ba.getElementAtIndex(1);
1029         final double biasZ = ba.getElementAtIndex(2);
1030 
1031         final KnownGravityNormAccelerometerCalibrator calibrator =
1032                 new KnownGravityNormAccelerometerCalibrator(measurements,
1033                         biasX, biasY, biasZ);
1034 
1035         // check default values
1036         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1037         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1038         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1039         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1040         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1041         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1042         final Acceleration bx2 = new Acceleration(0.0,
1043                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1044         calibrator.getInitialBiasXAsAcceleration(bx2);
1045         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1046         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1047         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1048         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1049         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1050         final Acceleration by2 = new Acceleration(0.0,
1051                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1052         calibrator.getInitialBiasYAsAcceleration(by2);
1053         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1054         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1055         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1056         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1057         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1058         final Acceleration bz2 = new Acceleration(0.0,
1059                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1060         calibrator.getInitialBiasZAsAcceleration(bz2);
1061         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1062         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1063         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1064         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1065         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1066         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1067         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1068         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1069         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1070         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1071         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1072         final double[] bias1 = calibrator.getInitialBias();
1073         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1074         final double[] bias2 = new double[3];
1075         calibrator.getInitialBias(bias2);
1076         assertArrayEquals(bias1, bias2, 0.0);
1077         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1078         assertEquals(b1, ba);
1079         final Matrix b2 = new Matrix(3, 1);
1080         calibrator.getInitialBiasAsMatrix(b2);
1081         assertEquals(b1, b2);
1082         final Matrix ma1 = calibrator.getInitialMa();
1083         assertEquals(ma1, new Matrix(3, 3));
1084         final Matrix ma2 = new Matrix(3, 3);
1085         calibrator.getInitialMa(ma2);
1086         assertEquals(ma1, ma2);
1087         assertSame(calibrator.getMeasurements(), measurements);
1088         assertFalse(calibrator.isCommonAxisUsed());
1089         assertNull(calibrator.getListener());
1090         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1091         assertFalse(calibrator.isReady());
1092         assertFalse(calibrator.isRunning());
1093         assertNull(calibrator.getEstimatedBiases());
1094         assertFalse(calibrator.getEstimatedBiases(null));
1095         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1096         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1097         assertNull(calibrator.getEstimatedBiasFx());
1098         assertNull(calibrator.getEstimatedBiasFy());
1099         assertNull(calibrator.getEstimatedBiasFz());
1100         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1101         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1102         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1103         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1104         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1105         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1106         assertNull(calibrator.getEstimatedMa());
1107         assertNull(calibrator.getEstimatedSx());
1108         assertNull(calibrator.getEstimatedSy());
1109         assertNull(calibrator.getEstimatedSz());
1110         assertNull(calibrator.getEstimatedMxy());
1111         assertNull(calibrator.getEstimatedMxz());
1112         assertNull(calibrator.getEstimatedMyx());
1113         assertNull(calibrator.getEstimatedMyz());
1114         assertNull(calibrator.getEstimatedMzx());
1115         assertNull(calibrator.getEstimatedMzy());
1116         assertNull(calibrator.getEstimatedCovariance());
1117         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1118         assertNull(calibrator.getGroundTruthGravityNorm());
1119         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1120         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1121     }
1122 
1123     @Test
1124     public void testConstructor12() throws WrongSizeException {
1125         final Collection<StandardDeviationBodyKinematics> measurements =
1126                 Collections.emptyList();
1127 
1128         final Matrix ba = generateBa();
1129         final double biasX = ba.getElementAtIndex(0);
1130         final double biasY = ba.getElementAtIndex(1);
1131         final double biasZ = ba.getElementAtIndex(2);
1132 
1133         final KnownGravityNormAccelerometerCalibrator calibrator =
1134                 new KnownGravityNormAccelerometerCalibrator(measurements,
1135                         biasX, biasY, biasZ, this);
1136 
1137         // check default values
1138         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1139         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1140         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1141         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1142         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1143         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1144         final Acceleration bx2 = new Acceleration(0.0,
1145                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1146         calibrator.getInitialBiasXAsAcceleration(bx2);
1147         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1148         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1149         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1150         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1151         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1152         final Acceleration by2 = new Acceleration(0.0,
1153                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1154         calibrator.getInitialBiasYAsAcceleration(by2);
1155         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1156         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1157         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1158         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1159         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1160         final Acceleration bz2 = new Acceleration(0.0,
1161                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1162         calibrator.getInitialBiasZAsAcceleration(bz2);
1163         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1164         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1165         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1166         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1167         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1168         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1169         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1170         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1171         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1172         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1173         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1174         final double[] bias1 = calibrator.getInitialBias();
1175         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1176         final double[] bias2 = new double[3];
1177         calibrator.getInitialBias(bias2);
1178         assertArrayEquals(bias1, bias2, 0.0);
1179         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1180         assertEquals(b1, ba);
1181         final Matrix b2 = new Matrix(3, 1);
1182         calibrator.getInitialBiasAsMatrix(b2);
1183         assertEquals(b1, b2);
1184         final Matrix ma1 = calibrator.getInitialMa();
1185         assertEquals(ma1, new Matrix(3, 3));
1186         final Matrix ma2 = new Matrix(3, 3);
1187         calibrator.getInitialMa(ma2);
1188         assertEquals(ma1, ma2);
1189         assertSame(calibrator.getMeasurements(), measurements);
1190         assertFalse(calibrator.isCommonAxisUsed());
1191         assertSame(calibrator.getListener(), this);
1192         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1193         assertFalse(calibrator.isReady());
1194         assertFalse(calibrator.isRunning());
1195         assertNull(calibrator.getEstimatedBiases());
1196         assertFalse(calibrator.getEstimatedBiases(null));
1197         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1198         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1199         assertNull(calibrator.getEstimatedBiasFx());
1200         assertNull(calibrator.getEstimatedBiasFy());
1201         assertNull(calibrator.getEstimatedBiasFz());
1202         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1203         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1204         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1205         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1206         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1207         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1208         assertNull(calibrator.getEstimatedMa());
1209         assertNull(calibrator.getEstimatedSx());
1210         assertNull(calibrator.getEstimatedSy());
1211         assertNull(calibrator.getEstimatedSz());
1212         assertNull(calibrator.getEstimatedMxy());
1213         assertNull(calibrator.getEstimatedMxz());
1214         assertNull(calibrator.getEstimatedMyx());
1215         assertNull(calibrator.getEstimatedMyz());
1216         assertNull(calibrator.getEstimatedMzx());
1217         assertNull(calibrator.getEstimatedMzy());
1218         assertNull(calibrator.getEstimatedCovariance());
1219         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1220         assertNull(calibrator.getGroundTruthGravityNorm());
1221         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1222         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1223     }
1224 
1225     @Test
1226     public void testConstructor13() throws WrongSizeException {
1227         final Matrix ba = generateBa();
1228         final double biasX = ba.getElementAtIndex(0);
1229         final double biasY = ba.getElementAtIndex(1);
1230         final double biasZ = ba.getElementAtIndex(2);
1231 
1232         final KnownGravityNormAccelerometerCalibrator calibrator =
1233                 new KnownGravityNormAccelerometerCalibrator(
1234                         true, biasX, biasY, biasZ);
1235 
1236         // check default values
1237         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1238         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1239         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1240         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1241         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1242         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1243         final Acceleration bx2 = new Acceleration(0.0,
1244                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1245         calibrator.getInitialBiasXAsAcceleration(bx2);
1246         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1247         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1248         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1249         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1250         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1251         final Acceleration by2 = new Acceleration(0.0,
1252                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1253         calibrator.getInitialBiasYAsAcceleration(by2);
1254         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1255         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1256         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1257         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1258         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1259         final Acceleration bz2 = new Acceleration(0.0,
1260                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1261         calibrator.getInitialBiasZAsAcceleration(bz2);
1262         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1263         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1264         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1265         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1266         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1267         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1268         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1269         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1270         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1271         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1272         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1273         final double[] bias1 = calibrator.getInitialBias();
1274         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1275         final double[] bias2 = new double[3];
1276         calibrator.getInitialBias(bias2);
1277         assertArrayEquals(bias1, bias2, 0.0);
1278         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1279         assertEquals(b1, ba);
1280         final Matrix b2 = new Matrix(3, 1);
1281         calibrator.getInitialBiasAsMatrix(b2);
1282         assertEquals(b1, b2);
1283         final Matrix ma1 = calibrator.getInitialMa();
1284         assertEquals(ma1, new Matrix(3, 3));
1285         final Matrix ma2 = new Matrix(3, 3);
1286         calibrator.getInitialMa(ma2);
1287         assertEquals(ma1, ma2);
1288         assertNull(calibrator.getMeasurements());
1289         assertTrue(calibrator.isCommonAxisUsed());
1290         assertNull(calibrator.getListener());
1291         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1292         assertFalse(calibrator.isReady());
1293         assertFalse(calibrator.isRunning());
1294         assertNull(calibrator.getEstimatedBiases());
1295         assertFalse(calibrator.getEstimatedBiases(null));
1296         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1297         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1298         assertNull(calibrator.getEstimatedBiasFx());
1299         assertNull(calibrator.getEstimatedBiasFy());
1300         assertNull(calibrator.getEstimatedBiasFz());
1301         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1302         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1303         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1304         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1305         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1306         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1307         assertNull(calibrator.getEstimatedMa());
1308         assertNull(calibrator.getEstimatedSx());
1309         assertNull(calibrator.getEstimatedSy());
1310         assertNull(calibrator.getEstimatedSz());
1311         assertNull(calibrator.getEstimatedMxy());
1312         assertNull(calibrator.getEstimatedMxz());
1313         assertNull(calibrator.getEstimatedMyx());
1314         assertNull(calibrator.getEstimatedMyz());
1315         assertNull(calibrator.getEstimatedMzx());
1316         assertNull(calibrator.getEstimatedMzy());
1317         assertNull(calibrator.getEstimatedCovariance());
1318         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1319         assertNull(calibrator.getGroundTruthGravityNorm());
1320         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1321         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1322     }
1323 
1324     @Test
1325     public void testConstructor14() throws WrongSizeException {
1326         final Matrix ba = generateBa();
1327         final double biasX = ba.getElementAtIndex(0);
1328         final double biasY = ba.getElementAtIndex(1);
1329         final double biasZ = ba.getElementAtIndex(2);
1330 
1331         final KnownGravityNormAccelerometerCalibrator calibrator =
1332                 new KnownGravityNormAccelerometerCalibrator(
1333                         true, biasX, biasY, biasZ,
1334                         this);
1335 
1336         // check default values
1337         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1338         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1339         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1340         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1341         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1342         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1343         final Acceleration bx2 = new Acceleration(0.0,
1344                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1345         calibrator.getInitialBiasXAsAcceleration(bx2);
1346         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1347         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1348         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1349         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1350         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1351         final Acceleration by2 = new Acceleration(0.0,
1352                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1353         calibrator.getInitialBiasYAsAcceleration(by2);
1354         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1355         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1356         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1357         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1358         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1359         final Acceleration bz2 = new Acceleration(0.0,
1360                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1361         calibrator.getInitialBiasZAsAcceleration(bz2);
1362         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1363         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1364         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1365         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1366         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1367         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1368         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1369         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1370         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1371         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1372         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1373         final double[] bias1 = calibrator.getInitialBias();
1374         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1375         final double[] bias2 = new double[3];
1376         calibrator.getInitialBias(bias2);
1377         assertArrayEquals(bias1, bias2, 0.0);
1378         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1379         assertEquals(b1, ba);
1380         final Matrix b2 = new Matrix(3, 1);
1381         calibrator.getInitialBiasAsMatrix(b2);
1382         assertEquals(b1, b2);
1383         final Matrix ma1 = calibrator.getInitialMa();
1384         assertEquals(ma1, new Matrix(3, 3));
1385         final Matrix ma2 = new Matrix(3, 3);
1386         calibrator.getInitialMa(ma2);
1387         assertEquals(ma1, ma2);
1388         assertNull(calibrator.getMeasurements());
1389         assertTrue(calibrator.isCommonAxisUsed());
1390         assertSame(calibrator.getListener(), this);
1391         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1392         assertFalse(calibrator.isReady());
1393         assertFalse(calibrator.isRunning());
1394         assertNull(calibrator.getEstimatedBiases());
1395         assertFalse(calibrator.getEstimatedBiases(null));
1396         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1397         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1398         assertNull(calibrator.getEstimatedBiasFx());
1399         assertNull(calibrator.getEstimatedBiasFy());
1400         assertNull(calibrator.getEstimatedBiasFz());
1401         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1402         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1403         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1404         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1405         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1406         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1407         assertNull(calibrator.getEstimatedMa());
1408         assertNull(calibrator.getEstimatedSx());
1409         assertNull(calibrator.getEstimatedSy());
1410         assertNull(calibrator.getEstimatedSz());
1411         assertNull(calibrator.getEstimatedMxy());
1412         assertNull(calibrator.getEstimatedMxz());
1413         assertNull(calibrator.getEstimatedMyx());
1414         assertNull(calibrator.getEstimatedMyz());
1415         assertNull(calibrator.getEstimatedMzx());
1416         assertNull(calibrator.getEstimatedMzy());
1417         assertNull(calibrator.getEstimatedCovariance());
1418         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1419         assertNull(calibrator.getGroundTruthGravityNorm());
1420         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1421         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1422     }
1423 
1424     @Test
1425     public void testConstructor15() throws WrongSizeException {
1426         final Collection<StandardDeviationBodyKinematics> measurements =
1427                 Collections.emptyList();
1428 
1429         final Matrix ba = generateBa();
1430         final double biasX = ba.getElementAtIndex(0);
1431         final double biasY = ba.getElementAtIndex(1);
1432         final double biasZ = ba.getElementAtIndex(2);
1433 
1434         final KnownGravityNormAccelerometerCalibrator calibrator =
1435                 new KnownGravityNormAccelerometerCalibrator(measurements,
1436                         true, biasX, biasY, biasZ);
1437 
1438         // check default values
1439         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1440         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1441         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1442         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1443         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1444         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1445         final Acceleration bx2 = new Acceleration(0.0,
1446                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1447         calibrator.getInitialBiasXAsAcceleration(bx2);
1448         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1449         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1450         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1451         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1452         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1453         final Acceleration by2 = new Acceleration(0.0,
1454                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1455         calibrator.getInitialBiasYAsAcceleration(by2);
1456         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1457         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1458         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1459         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1460         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1461         final Acceleration bz2 = new Acceleration(0.0,
1462                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1463         calibrator.getInitialBiasZAsAcceleration(bz2);
1464         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1465         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1466         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1467         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1468         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1469         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1470         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1471         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1472         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1473         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1474         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1475         final double[] bias1 = calibrator.getInitialBias();
1476         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1477         final double[] bias2 = new double[3];
1478         calibrator.getInitialBias(bias2);
1479         assertArrayEquals(bias1, bias2, 0.0);
1480         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1481         assertEquals(b1, ba);
1482         final Matrix b2 = new Matrix(3, 1);
1483         calibrator.getInitialBiasAsMatrix(b2);
1484         assertEquals(b1, b2);
1485         final Matrix ma1 = calibrator.getInitialMa();
1486         assertEquals(ma1, new Matrix(3, 3));
1487         final Matrix ma2 = new Matrix(3, 3);
1488         calibrator.getInitialMa(ma2);
1489         assertEquals(ma1, ma2);
1490         assertSame(calibrator.getMeasurements(), measurements);
1491         assertTrue(calibrator.isCommonAxisUsed());
1492         assertNull(calibrator.getListener());
1493         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1494         assertFalse(calibrator.isReady());
1495         assertFalse(calibrator.isRunning());
1496         assertNull(calibrator.getEstimatedBiases());
1497         assertFalse(calibrator.getEstimatedBiases(null));
1498         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1499         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1500         assertNull(calibrator.getEstimatedBiasFx());
1501         assertNull(calibrator.getEstimatedBiasFy());
1502         assertNull(calibrator.getEstimatedBiasFz());
1503         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1504         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1505         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1506         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1507         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1508         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1509         assertNull(calibrator.getEstimatedMa());
1510         assertNull(calibrator.getEstimatedSx());
1511         assertNull(calibrator.getEstimatedSy());
1512         assertNull(calibrator.getEstimatedSz());
1513         assertNull(calibrator.getEstimatedMxy());
1514         assertNull(calibrator.getEstimatedMxz());
1515         assertNull(calibrator.getEstimatedMyx());
1516         assertNull(calibrator.getEstimatedMyz());
1517         assertNull(calibrator.getEstimatedMzx());
1518         assertNull(calibrator.getEstimatedMzy());
1519         assertNull(calibrator.getEstimatedCovariance());
1520         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1521         assertNull(calibrator.getGroundTruthGravityNorm());
1522         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1523         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1524     }
1525 
1526     @Test
1527     public void testConstructor16() throws WrongSizeException {
1528         final Collection<StandardDeviationBodyKinematics> measurements =
1529                 Collections.emptyList();
1530 
1531         final Matrix ba = generateBa();
1532         final double biasX = ba.getElementAtIndex(0);
1533         final double biasY = ba.getElementAtIndex(1);
1534         final double biasZ = ba.getElementAtIndex(2);
1535 
1536         final KnownGravityNormAccelerometerCalibrator calibrator =
1537                 new KnownGravityNormAccelerometerCalibrator(measurements,
1538                         true, biasX, biasY, biasZ, this);
1539 
1540         // check default values
1541         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1542         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1543         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1544         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1545         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1546         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1547         final Acceleration bx2 = new Acceleration(0.0,
1548                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1549         calibrator.getInitialBiasXAsAcceleration(bx2);
1550         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1551         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1552         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1553         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1554         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1555         final Acceleration by2 = new Acceleration(0.0,
1556                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1557         calibrator.getInitialBiasYAsAcceleration(by2);
1558         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1559         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1560         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1561         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1562         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1563         final Acceleration bz2 = new Acceleration(0.0,
1564                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1565         calibrator.getInitialBiasZAsAcceleration(bz2);
1566         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1567         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1568         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1569         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1570         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1571         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1572         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1573         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1574         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1575         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1576         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1577         final double[] bias1 = calibrator.getInitialBias();
1578         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1579         final double[] bias2 = new double[3];
1580         calibrator.getInitialBias(bias2);
1581         assertArrayEquals(bias1, bias2, 0.0);
1582         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1583         assertEquals(b1, ba);
1584         final Matrix b2 = new Matrix(3, 1);
1585         calibrator.getInitialBiasAsMatrix(b2);
1586         assertEquals(b1, b2);
1587         final Matrix ma1 = calibrator.getInitialMa();
1588         assertEquals(ma1, new Matrix(3, 3));
1589         final Matrix ma2 = new Matrix(3, 3);
1590         calibrator.getInitialMa(ma2);
1591         assertEquals(ma1, ma2);
1592         assertSame(calibrator.getMeasurements(), measurements);
1593         assertTrue(calibrator.isCommonAxisUsed());
1594         assertSame(calibrator.getListener(), this);
1595         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1596         assertFalse(calibrator.isReady());
1597         assertFalse(calibrator.isRunning());
1598         assertNull(calibrator.getEstimatedBiases());
1599         assertFalse(calibrator.getEstimatedBiases(null));
1600         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1601         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1602         assertNull(calibrator.getEstimatedBiasFx());
1603         assertNull(calibrator.getEstimatedBiasFy());
1604         assertNull(calibrator.getEstimatedBiasFz());
1605         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1606         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1607         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1608         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1609         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1610         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1611         assertNull(calibrator.getEstimatedMa());
1612         assertNull(calibrator.getEstimatedSx());
1613         assertNull(calibrator.getEstimatedSy());
1614         assertNull(calibrator.getEstimatedSz());
1615         assertNull(calibrator.getEstimatedMxy());
1616         assertNull(calibrator.getEstimatedMxz());
1617         assertNull(calibrator.getEstimatedMyx());
1618         assertNull(calibrator.getEstimatedMyz());
1619         assertNull(calibrator.getEstimatedMzx());
1620         assertNull(calibrator.getEstimatedMzy());
1621         assertNull(calibrator.getEstimatedCovariance());
1622         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1623         assertNull(calibrator.getGroundTruthGravityNorm());
1624         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1625         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1626     }
1627 
1628     @Test
1629     public void testConstructor17() throws WrongSizeException {
1630         final Matrix ba = generateBa();
1631         final double biasX = ba.getElementAtIndex(0);
1632         final double biasY = ba.getElementAtIndex(1);
1633         final double biasZ = ba.getElementAtIndex(2);
1634 
1635         final Acceleration bx = new Acceleration(biasX,
1636                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1637         final Acceleration by = new Acceleration(biasY,
1638                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1639         final Acceleration bz = new Acceleration(biasZ,
1640                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1641 
1642         final KnownGravityNormAccelerometerCalibrator calibrator =
1643                 new KnownGravityNormAccelerometerCalibrator(bx, by, bz);
1644 
1645         // check default values
1646         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1647         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1648         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1649         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1650         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1651         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1652         final Acceleration bx2 = new Acceleration(0.0,
1653                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1654         calibrator.getInitialBiasXAsAcceleration(bx2);
1655         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1656         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1657         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1658         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1659         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1660         final Acceleration by2 = new Acceleration(0.0,
1661                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1662         calibrator.getInitialBiasYAsAcceleration(by2);
1663         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1664         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1665         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1666         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1667         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1668         final Acceleration bz2 = new Acceleration(0.0,
1669                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1670         calibrator.getInitialBiasZAsAcceleration(bz2);
1671         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1672         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1673         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1674         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1675         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1676         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1677         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1678         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1679         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1680         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1681         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1682         final double[] bias1 = calibrator.getInitialBias();
1683         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1684         final double[] bias2 = new double[3];
1685         calibrator.getInitialBias(bias2);
1686         assertArrayEquals(bias1, bias2, 0.0);
1687         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1688         assertEquals(b1, ba);
1689         final Matrix b2 = new Matrix(3, 1);
1690         calibrator.getInitialBiasAsMatrix(b2);
1691         assertEquals(b1, b2);
1692         final Matrix ma1 = calibrator.getInitialMa();
1693         assertEquals(ma1, new Matrix(3, 3));
1694         final Matrix ma2 = new Matrix(3, 3);
1695         calibrator.getInitialMa(ma2);
1696         assertEquals(ma1, ma2);
1697         assertNull(calibrator.getMeasurements());
1698         assertFalse(calibrator.isCommonAxisUsed());
1699         assertNull(calibrator.getListener());
1700         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1701         assertFalse(calibrator.isReady());
1702         assertFalse(calibrator.isRunning());
1703         assertNull(calibrator.getEstimatedBiases());
1704         assertFalse(calibrator.getEstimatedBiases(null));
1705         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1706         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1707         assertNull(calibrator.getEstimatedBiasFx());
1708         assertNull(calibrator.getEstimatedBiasFy());
1709         assertNull(calibrator.getEstimatedBiasFz());
1710         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1711         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1712         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1713         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1714         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1715         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1716         assertNull(calibrator.getEstimatedMa());
1717         assertNull(calibrator.getEstimatedSx());
1718         assertNull(calibrator.getEstimatedSy());
1719         assertNull(calibrator.getEstimatedSz());
1720         assertNull(calibrator.getEstimatedMxy());
1721         assertNull(calibrator.getEstimatedMxz());
1722         assertNull(calibrator.getEstimatedMyx());
1723         assertNull(calibrator.getEstimatedMyz());
1724         assertNull(calibrator.getEstimatedMzx());
1725         assertNull(calibrator.getEstimatedMzy());
1726         assertNull(calibrator.getEstimatedCovariance());
1727         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1728         assertNull(calibrator.getGroundTruthGravityNorm());
1729         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1730         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1731     }
1732 
1733     @Test
1734     public void testConstructor18() throws WrongSizeException {
1735         final Matrix ba = generateBa();
1736         final double biasX = ba.getElementAtIndex(0);
1737         final double biasY = ba.getElementAtIndex(1);
1738         final double biasZ = ba.getElementAtIndex(2);
1739 
1740         final Acceleration bx = new Acceleration(biasX,
1741                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1742         final Acceleration by = new Acceleration(biasY,
1743                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1744         final Acceleration bz = new Acceleration(biasZ,
1745                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1746 
1747         final KnownGravityNormAccelerometerCalibrator calibrator =
1748                 new KnownGravityNormAccelerometerCalibrator(bx, by, bz,
1749                         this);
1750 
1751         // check default values
1752         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1753         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1754         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1755         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1756         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1757         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1758         final Acceleration bx2 = new Acceleration(0.0,
1759                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1760         calibrator.getInitialBiasXAsAcceleration(bx2);
1761         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1762         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1763         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1764         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1765         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1766         final Acceleration by2 = new Acceleration(0.0,
1767                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1768         calibrator.getInitialBiasYAsAcceleration(by2);
1769         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1770         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1771         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1772         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1773         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1774         final Acceleration bz2 = new Acceleration(0.0,
1775                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1776         calibrator.getInitialBiasZAsAcceleration(bz2);
1777         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1778         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1779         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1780         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1781         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1782         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1783         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1784         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1785         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1786         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1787         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1788         final double[] bias1 = calibrator.getInitialBias();
1789         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1790         final double[] bias2 = new double[3];
1791         calibrator.getInitialBias(bias2);
1792         assertArrayEquals(bias1, bias2, 0.0);
1793         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1794         assertEquals(b1, ba);
1795         final Matrix b2 = new Matrix(3, 1);
1796         calibrator.getInitialBiasAsMatrix(b2);
1797         assertEquals(b1, b2);
1798         final Matrix ma1 = calibrator.getInitialMa();
1799         assertEquals(ma1, new Matrix(3, 3));
1800         final Matrix ma2 = new Matrix(3, 3);
1801         calibrator.getInitialMa(ma2);
1802         assertEquals(ma1, ma2);
1803         assertNull(calibrator.getMeasurements());
1804         assertFalse(calibrator.isCommonAxisUsed());
1805         assertSame(calibrator.getListener(), this);
1806         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1807         assertFalse(calibrator.isReady());
1808         assertFalse(calibrator.isRunning());
1809         assertNull(calibrator.getEstimatedBiases());
1810         assertFalse(calibrator.getEstimatedBiases(null));
1811         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1812         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1813         assertNull(calibrator.getEstimatedBiasFx());
1814         assertNull(calibrator.getEstimatedBiasFy());
1815         assertNull(calibrator.getEstimatedBiasFz());
1816         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1817         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1818         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1819         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1820         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1821         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1822         assertNull(calibrator.getEstimatedMa());
1823         assertNull(calibrator.getEstimatedSx());
1824         assertNull(calibrator.getEstimatedSy());
1825         assertNull(calibrator.getEstimatedSz());
1826         assertNull(calibrator.getEstimatedMxy());
1827         assertNull(calibrator.getEstimatedMxz());
1828         assertNull(calibrator.getEstimatedMyx());
1829         assertNull(calibrator.getEstimatedMyz());
1830         assertNull(calibrator.getEstimatedMzx());
1831         assertNull(calibrator.getEstimatedMzy());
1832         assertNull(calibrator.getEstimatedCovariance());
1833         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1834         assertNull(calibrator.getGroundTruthGravityNorm());
1835         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1836         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1837     }
1838 
1839     @Test
1840     public void testConstructor19() throws WrongSizeException {
1841         final Collection<StandardDeviationBodyKinematics> measurements =
1842                 Collections.emptyList();
1843 
1844         final Matrix ba = generateBa();
1845         final double biasX = ba.getElementAtIndex(0);
1846         final double biasY = ba.getElementAtIndex(1);
1847         final double biasZ = ba.getElementAtIndex(2);
1848 
1849         final Acceleration bx = new Acceleration(biasX,
1850                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1851         final Acceleration by = new Acceleration(biasY,
1852                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1853         final Acceleration bz = new Acceleration(biasZ,
1854                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1855 
1856         final KnownGravityNormAccelerometerCalibrator calibrator =
1857                 new KnownGravityNormAccelerometerCalibrator(measurements,
1858                         bx, by, bz);
1859 
1860         // check default values
1861         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1862         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1863         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1864         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1865         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1866         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1867         final Acceleration bx2 = new Acceleration(0.0,
1868                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1869         calibrator.getInitialBiasXAsAcceleration(bx2);
1870         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1871         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1872         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1873         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1874         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1875         final Acceleration by2 = new Acceleration(0.0,
1876                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1877         calibrator.getInitialBiasYAsAcceleration(by2);
1878         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1879         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1880         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1881         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1882         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1883         final Acceleration bz2 = new Acceleration(0.0,
1884                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1885         calibrator.getInitialBiasZAsAcceleration(bz2);
1886         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1887         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1888         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1889         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1890         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1891         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1892         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1893         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1894         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1895         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1896         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1897         final double[] bias1 = calibrator.getInitialBias();
1898         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1899         final double[] bias2 = new double[3];
1900         calibrator.getInitialBias(bias2);
1901         assertArrayEquals(bias1, bias2, 0.0);
1902         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1903         assertEquals(b1, ba);
1904         final Matrix b2 = new Matrix(3, 1);
1905         calibrator.getInitialBiasAsMatrix(b2);
1906         assertEquals(b1, b2);
1907         final Matrix ma1 = calibrator.getInitialMa();
1908         assertEquals(ma1, new Matrix(3, 3));
1909         final Matrix ma2 = new Matrix(3, 3);
1910         calibrator.getInitialMa(ma2);
1911         assertEquals(ma1, ma2);
1912         assertSame(calibrator.getMeasurements(), measurements);
1913         assertFalse(calibrator.isCommonAxisUsed());
1914         assertNull(calibrator.getListener());
1915         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1916         assertFalse(calibrator.isReady());
1917         assertFalse(calibrator.isRunning());
1918         assertNull(calibrator.getEstimatedBiases());
1919         assertFalse(calibrator.getEstimatedBiases(null));
1920         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1921         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1922         assertNull(calibrator.getEstimatedBiasFx());
1923         assertNull(calibrator.getEstimatedBiasFy());
1924         assertNull(calibrator.getEstimatedBiasFz());
1925         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1926         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1927         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1928         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1929         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1930         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1931         assertNull(calibrator.getEstimatedMa());
1932         assertNull(calibrator.getEstimatedSx());
1933         assertNull(calibrator.getEstimatedSy());
1934         assertNull(calibrator.getEstimatedSz());
1935         assertNull(calibrator.getEstimatedMxy());
1936         assertNull(calibrator.getEstimatedMxz());
1937         assertNull(calibrator.getEstimatedMyx());
1938         assertNull(calibrator.getEstimatedMyz());
1939         assertNull(calibrator.getEstimatedMzx());
1940         assertNull(calibrator.getEstimatedMzy());
1941         assertNull(calibrator.getEstimatedCovariance());
1942         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1943         assertNull(calibrator.getGroundTruthGravityNorm());
1944         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1945         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1946     }
1947 
1948     @Test
1949     public void testConstructor20() throws WrongSizeException {
1950         final Collection<StandardDeviationBodyKinematics> measurements =
1951                 Collections.emptyList();
1952 
1953         final Matrix ba = generateBa();
1954         final double biasX = ba.getElementAtIndex(0);
1955         final double biasY = ba.getElementAtIndex(1);
1956         final double biasZ = ba.getElementAtIndex(2);
1957 
1958         final Acceleration bx = new Acceleration(biasX,
1959                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1960         final Acceleration by = new Acceleration(biasY,
1961                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1962         final Acceleration bz = new Acceleration(biasZ,
1963                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1964 
1965         final KnownGravityNormAccelerometerCalibrator calibrator =
1966                 new KnownGravityNormAccelerometerCalibrator(measurements,
1967                         bx, by, bz, this);
1968 
1969         // check default values
1970         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1971         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1972         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1973         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1974         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1975         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1976         final Acceleration bx2 = new Acceleration(0.0,
1977                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1978         calibrator.getInitialBiasXAsAcceleration(bx2);
1979         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1980         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1981         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1982         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1983         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1984         final Acceleration by2 = new Acceleration(0.0,
1985                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1986         calibrator.getInitialBiasYAsAcceleration(by2);
1987         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1988         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1989         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1990         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1991         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1992         final Acceleration bz2 = new Acceleration(0.0,
1993                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1994         calibrator.getInitialBiasZAsAcceleration(bz2);
1995         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1996         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1997         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1998         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1999         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2000         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2001         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2002         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2003         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2004         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2005         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2006         final double[] bias1 = calibrator.getInitialBias();
2007         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2008         final double[] bias2 = new double[3];
2009         calibrator.getInitialBias(bias2);
2010         assertArrayEquals(bias1, bias2, 0.0);
2011         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2012         assertEquals(b1, ba);
2013         final Matrix b2 = new Matrix(3, 1);
2014         calibrator.getInitialBiasAsMatrix(b2);
2015         assertEquals(b1, b2);
2016         final Matrix ma1 = calibrator.getInitialMa();
2017         assertEquals(ma1, new Matrix(3, 3));
2018         final Matrix ma2 = new Matrix(3, 3);
2019         calibrator.getInitialMa(ma2);
2020         assertEquals(ma1, ma2);
2021         assertSame(calibrator.getMeasurements(), measurements);
2022         assertFalse(calibrator.isCommonAxisUsed());
2023         assertSame(calibrator.getListener(), this);
2024         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2025         assertFalse(calibrator.isReady());
2026         assertFalse(calibrator.isRunning());
2027         assertNull(calibrator.getEstimatedBiases());
2028         assertFalse(calibrator.getEstimatedBiases(null));
2029         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2030         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2031         assertNull(calibrator.getEstimatedBiasFx());
2032         assertNull(calibrator.getEstimatedBiasFy());
2033         assertNull(calibrator.getEstimatedBiasFz());
2034         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2035         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2036         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2037         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2038         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2039         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2040         assertNull(calibrator.getEstimatedMa());
2041         assertNull(calibrator.getEstimatedSx());
2042         assertNull(calibrator.getEstimatedSy());
2043         assertNull(calibrator.getEstimatedSz());
2044         assertNull(calibrator.getEstimatedMxy());
2045         assertNull(calibrator.getEstimatedMxz());
2046         assertNull(calibrator.getEstimatedMyx());
2047         assertNull(calibrator.getEstimatedMyz());
2048         assertNull(calibrator.getEstimatedMzx());
2049         assertNull(calibrator.getEstimatedMzy());
2050         assertNull(calibrator.getEstimatedCovariance());
2051         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2052         assertNull(calibrator.getGroundTruthGravityNorm());
2053         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2054         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2055     }
2056 
2057     @Test
2058     public void testConstructor21() throws WrongSizeException {
2059         final Matrix ba = generateBa();
2060         final double biasX = ba.getElementAtIndex(0);
2061         final double biasY = ba.getElementAtIndex(1);
2062         final double biasZ = ba.getElementAtIndex(2);
2063 
2064         final Acceleration bx = new Acceleration(biasX,
2065                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2066         final Acceleration by = new Acceleration(biasY,
2067                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2068         final Acceleration bz = new Acceleration(biasZ,
2069                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2070 
2071         final KnownGravityNormAccelerometerCalibrator calibrator =
2072                 new KnownGravityNormAccelerometerCalibrator(
2073                         true, bx, by, bz);
2074 
2075         // check default values
2076         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2077         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2078         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2079         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2080         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2081         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2082         final Acceleration bx2 = new Acceleration(0.0,
2083                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2084         calibrator.getInitialBiasXAsAcceleration(bx2);
2085         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2086         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2087         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2088         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2089         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2090         final Acceleration by2 = new Acceleration(0.0,
2091                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2092         calibrator.getInitialBiasYAsAcceleration(by2);
2093         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2094         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2095         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2096         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2097         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2098         final Acceleration bz2 = new Acceleration(0.0,
2099                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2100         calibrator.getInitialBiasZAsAcceleration(bz2);
2101         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2102         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2103         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2104         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2105         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2106         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2107         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2108         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2109         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2110         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2111         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2112         final double[] bias1 = calibrator.getInitialBias();
2113         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2114         final double[] bias2 = new double[3];
2115         calibrator.getInitialBias(bias2);
2116         assertArrayEquals(bias1, bias2, 0.0);
2117         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2118         assertEquals(b1, ba);
2119         final Matrix b2 = new Matrix(3, 1);
2120         calibrator.getInitialBiasAsMatrix(b2);
2121         assertEquals(b1, b2);
2122         final Matrix ma1 = calibrator.getInitialMa();
2123         assertEquals(ma1, new Matrix(3, 3));
2124         final Matrix ma2 = new Matrix(3, 3);
2125         calibrator.getInitialMa(ma2);
2126         assertEquals(ma1, ma2);
2127         assertNull(calibrator.getMeasurements());
2128         assertTrue(calibrator.isCommonAxisUsed());
2129         assertNull(calibrator.getListener());
2130         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2131         assertFalse(calibrator.isReady());
2132         assertFalse(calibrator.isRunning());
2133         assertNull(calibrator.getEstimatedBiases());
2134         assertFalse(calibrator.getEstimatedBiases(null));
2135         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2136         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2137         assertNull(calibrator.getEstimatedBiasFx());
2138         assertNull(calibrator.getEstimatedBiasFy());
2139         assertNull(calibrator.getEstimatedBiasFz());
2140         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2141         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2142         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2143         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2144         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2145         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2146         assertNull(calibrator.getEstimatedMa());
2147         assertNull(calibrator.getEstimatedSx());
2148         assertNull(calibrator.getEstimatedSy());
2149         assertNull(calibrator.getEstimatedSz());
2150         assertNull(calibrator.getEstimatedMxy());
2151         assertNull(calibrator.getEstimatedMxz());
2152         assertNull(calibrator.getEstimatedMyx());
2153         assertNull(calibrator.getEstimatedMyz());
2154         assertNull(calibrator.getEstimatedMzx());
2155         assertNull(calibrator.getEstimatedMzy());
2156         assertNull(calibrator.getEstimatedCovariance());
2157         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2158         assertNull(calibrator.getGroundTruthGravityNorm());
2159         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2160         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2161     }
2162 
2163     @Test
2164     public void testConstructor22() throws WrongSizeException {
2165         final Matrix ba = generateBa();
2166         final double biasX = ba.getElementAtIndex(0);
2167         final double biasY = ba.getElementAtIndex(1);
2168         final double biasZ = ba.getElementAtIndex(2);
2169 
2170         final Acceleration bx = new Acceleration(biasX,
2171                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2172         final Acceleration by = new Acceleration(biasY,
2173                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2174         final Acceleration bz = new Acceleration(biasZ,
2175                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2176 
2177         final KnownGravityNormAccelerometerCalibrator calibrator =
2178                 new KnownGravityNormAccelerometerCalibrator(
2179                         true, bx, by, bz, this);
2180 
2181         // check default values
2182         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2183         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2184         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2185         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2186         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2187         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2188         final Acceleration bx2 = new Acceleration(0.0,
2189                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2190         calibrator.getInitialBiasXAsAcceleration(bx2);
2191         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2192         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2193         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2194         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2195         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2196         final Acceleration by2 = new Acceleration(0.0,
2197                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2198         calibrator.getInitialBiasYAsAcceleration(by2);
2199         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2200         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2201         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2202         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2203         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2204         final Acceleration bz2 = new Acceleration(0.0,
2205                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2206         calibrator.getInitialBiasZAsAcceleration(bz2);
2207         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2208         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2209         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2210         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2211         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2212         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2213         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2214         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2215         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2216         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2217         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2218         final double[] bias1 = calibrator.getInitialBias();
2219         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2220         final double[] bias2 = new double[3];
2221         calibrator.getInitialBias(bias2);
2222         assertArrayEquals(bias1, bias2, 0.0);
2223         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2224         assertEquals(b1, ba);
2225         final Matrix b2 = new Matrix(3, 1);
2226         calibrator.getInitialBiasAsMatrix(b2);
2227         assertEquals(b1, b2);
2228         final Matrix ma1 = calibrator.getInitialMa();
2229         assertEquals(ma1, new Matrix(3, 3));
2230         final Matrix ma2 = new Matrix(3, 3);
2231         calibrator.getInitialMa(ma2);
2232         assertEquals(ma1, ma2);
2233         assertNull(calibrator.getMeasurements());
2234         assertTrue(calibrator.isCommonAxisUsed());
2235         assertSame(calibrator.getListener(), this);
2236         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2237         assertFalse(calibrator.isReady());
2238         assertFalse(calibrator.isRunning());
2239         assertNull(calibrator.getEstimatedBiases());
2240         assertFalse(calibrator.getEstimatedBiases(null));
2241         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2242         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2243         assertNull(calibrator.getEstimatedBiasFx());
2244         assertNull(calibrator.getEstimatedBiasFy());
2245         assertNull(calibrator.getEstimatedBiasFz());
2246         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2247         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2248         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2249         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2250         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2251         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2252         assertNull(calibrator.getEstimatedMa());
2253         assertNull(calibrator.getEstimatedSx());
2254         assertNull(calibrator.getEstimatedSy());
2255         assertNull(calibrator.getEstimatedSz());
2256         assertNull(calibrator.getEstimatedMxy());
2257         assertNull(calibrator.getEstimatedMxz());
2258         assertNull(calibrator.getEstimatedMyx());
2259         assertNull(calibrator.getEstimatedMyz());
2260         assertNull(calibrator.getEstimatedMzx());
2261         assertNull(calibrator.getEstimatedMzy());
2262         assertNull(calibrator.getEstimatedCovariance());
2263         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2264         assertNull(calibrator.getGroundTruthGravityNorm());
2265         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2266         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2267     }
2268 
2269     @Test
2270     public void testConstructor23() throws WrongSizeException {
2271         final Collection<StandardDeviationBodyKinematics> measurements =
2272                 Collections.emptyList();
2273 
2274         final Matrix ba = generateBa();
2275         final double biasX = ba.getElementAtIndex(0);
2276         final double biasY = ba.getElementAtIndex(1);
2277         final double biasZ = ba.getElementAtIndex(2);
2278 
2279         final Acceleration bx = new Acceleration(biasX,
2280                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2281         final Acceleration by = new Acceleration(biasY,
2282                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2283         final Acceleration bz = new Acceleration(biasZ,
2284                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2285 
2286         final KnownGravityNormAccelerometerCalibrator calibrator =
2287                 new KnownGravityNormAccelerometerCalibrator(measurements,
2288                         true, bx, by, bz);
2289 
2290         // check default values
2291         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2292         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2293         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2294         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2295         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2296         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2297         final Acceleration bx2 = new Acceleration(0.0,
2298                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2299         calibrator.getInitialBiasXAsAcceleration(bx2);
2300         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2301         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2302         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2303         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2304         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2305         final Acceleration by2 = new Acceleration(0.0,
2306                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2307         calibrator.getInitialBiasYAsAcceleration(by2);
2308         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2309         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2310         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2311         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2312         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2313         final Acceleration bz2 = new Acceleration(0.0,
2314                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2315         calibrator.getInitialBiasZAsAcceleration(bz2);
2316         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2317         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2318         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2319         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2320         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2321         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2322         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2323         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2324         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2325         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2326         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2327         final double[] bias1 = calibrator.getInitialBias();
2328         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2329         final double[] bias2 = new double[3];
2330         calibrator.getInitialBias(bias2);
2331         assertArrayEquals(bias1, bias2, 0.0);
2332         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2333         assertEquals(b1, ba);
2334         final Matrix b2 = new Matrix(3, 1);
2335         calibrator.getInitialBiasAsMatrix(b2);
2336         assertEquals(b1, b2);
2337         final Matrix ma1 = calibrator.getInitialMa();
2338         assertEquals(ma1, new Matrix(3, 3));
2339         final Matrix ma2 = new Matrix(3, 3);
2340         calibrator.getInitialMa(ma2);
2341         assertEquals(ma1, ma2);
2342         assertSame(calibrator.getMeasurements(), measurements);
2343         assertTrue(calibrator.isCommonAxisUsed());
2344         assertNull(calibrator.getListener());
2345         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2346         assertFalse(calibrator.isReady());
2347         assertFalse(calibrator.isRunning());
2348         assertNull(calibrator.getEstimatedBiases());
2349         assertFalse(calibrator.getEstimatedBiases(null));
2350         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2351         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2352         assertNull(calibrator.getEstimatedBiasFx());
2353         assertNull(calibrator.getEstimatedBiasFy());
2354         assertNull(calibrator.getEstimatedBiasFz());
2355         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2356         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2357         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2358         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2359         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2360         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2361         assertNull(calibrator.getEstimatedMa());
2362         assertNull(calibrator.getEstimatedSx());
2363         assertNull(calibrator.getEstimatedSy());
2364         assertNull(calibrator.getEstimatedSz());
2365         assertNull(calibrator.getEstimatedMxy());
2366         assertNull(calibrator.getEstimatedMxz());
2367         assertNull(calibrator.getEstimatedMyx());
2368         assertNull(calibrator.getEstimatedMyz());
2369         assertNull(calibrator.getEstimatedMzx());
2370         assertNull(calibrator.getEstimatedMzy());
2371         assertNull(calibrator.getEstimatedCovariance());
2372         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2373         assertNull(calibrator.getGroundTruthGravityNorm());
2374         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2375         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2376     }
2377 
2378     @Test
2379     public void testConstructor24() throws WrongSizeException {
2380         final Collection<StandardDeviationBodyKinematics> measurements =
2381                 Collections.emptyList();
2382 
2383         final Matrix ba = generateBa();
2384         final double biasX = ba.getElementAtIndex(0);
2385         final double biasY = ba.getElementAtIndex(1);
2386         final double biasZ = ba.getElementAtIndex(2);
2387 
2388         final Acceleration bx = new Acceleration(biasX,
2389                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2390         final Acceleration by = new Acceleration(biasY,
2391                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2392         final Acceleration bz = new Acceleration(biasZ,
2393                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2394 
2395         final KnownGravityNormAccelerometerCalibrator calibrator =
2396                 new KnownGravityNormAccelerometerCalibrator(measurements,
2397                         true, bx, by, bz, this);
2398 
2399         // check default values
2400         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2401         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2402         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2403         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2404         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2405         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2406         final Acceleration bx2 = new Acceleration(0.0,
2407                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2408         calibrator.getInitialBiasXAsAcceleration(bx2);
2409         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2410         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2411         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2412         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2413         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2414         final Acceleration by2 = new Acceleration(0.0,
2415                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2416         calibrator.getInitialBiasYAsAcceleration(by2);
2417         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2418         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2419         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2420         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2421         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2422         final Acceleration bz2 = new Acceleration(0.0,
2423                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2424         calibrator.getInitialBiasZAsAcceleration(bz2);
2425         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2426         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2427         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2428         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2429         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2430         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2431         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2432         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2433         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2434         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2435         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2436         final double[] bias1 = calibrator.getInitialBias();
2437         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2438         final double[] bias2 = new double[3];
2439         calibrator.getInitialBias(bias2);
2440         assertArrayEquals(bias1, bias2, 0.0);
2441         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2442         assertEquals(b1, ba);
2443         final Matrix b2 = new Matrix(3, 1);
2444         calibrator.getInitialBiasAsMatrix(b2);
2445         assertEquals(b1, b2);
2446         final Matrix ma1 = calibrator.getInitialMa();
2447         assertEquals(ma1, new Matrix(3, 3));
2448         final Matrix ma2 = new Matrix(3, 3);
2449         calibrator.getInitialMa(ma2);
2450         assertEquals(ma1, ma2);
2451         assertSame(calibrator.getMeasurements(), measurements);
2452         assertTrue(calibrator.isCommonAxisUsed());
2453         assertSame(calibrator.getListener(), this);
2454         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2455         assertFalse(calibrator.isReady());
2456         assertFalse(calibrator.isRunning());
2457         assertNull(calibrator.getEstimatedBiases());
2458         assertFalse(calibrator.getEstimatedBiases(null));
2459         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2460         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2461         assertNull(calibrator.getEstimatedBiasFx());
2462         assertNull(calibrator.getEstimatedBiasFy());
2463         assertNull(calibrator.getEstimatedBiasFz());
2464         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2465         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2466         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2467         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2468         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2469         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2470         assertNull(calibrator.getEstimatedMa());
2471         assertNull(calibrator.getEstimatedSx());
2472         assertNull(calibrator.getEstimatedSy());
2473         assertNull(calibrator.getEstimatedSz());
2474         assertNull(calibrator.getEstimatedMxy());
2475         assertNull(calibrator.getEstimatedMxz());
2476         assertNull(calibrator.getEstimatedMyx());
2477         assertNull(calibrator.getEstimatedMyz());
2478         assertNull(calibrator.getEstimatedMzx());
2479         assertNull(calibrator.getEstimatedMzy());
2480         assertNull(calibrator.getEstimatedCovariance());
2481         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2482         assertNull(calibrator.getGroundTruthGravityNorm());
2483         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2484         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2485     }
2486 
2487     @Test
2488     public void testConstructor25() throws WrongSizeException {
2489         final Matrix ba = generateBa();
2490         final double biasX = ba.getElementAtIndex(0);
2491         final double biasY = ba.getElementAtIndex(1);
2492         final double biasZ = ba.getElementAtIndex(2);
2493 
2494         final Matrix ma = generateMaCommonAxis();
2495         final double sx = ma.getElementAt(0, 0);
2496         final double sy = ma.getElementAt(1, 1);
2497         final double sz = ma.getElementAt(2, 2);
2498 
2499         final KnownGravityNormAccelerometerCalibrator calibrator =
2500                 new KnownGravityNormAccelerometerCalibrator(
2501                         biasX, biasY, biasZ,
2502                         sx, sy, sz);
2503 
2504         // check default values
2505         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2506         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2507         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2508         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2509         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2510         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2511         final Acceleration bx2 = new Acceleration(0.0,
2512                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2513         calibrator.getInitialBiasXAsAcceleration(bx2);
2514         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2515         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2516         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2517         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2518         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2519         final Acceleration by2 = new Acceleration(0.0,
2520                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2521         calibrator.getInitialBiasYAsAcceleration(by2);
2522         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2523         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2524         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2525         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2526         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2527         final Acceleration bz2 = new Acceleration(0.0,
2528                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2529         calibrator.getInitialBiasZAsAcceleration(bz2);
2530         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2531         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2532         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2533         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2534         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2535         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2536         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2537         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2538         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2539         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2540         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2541         final double[] bias1 = calibrator.getInitialBias();
2542         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2543         final double[] bias2 = new double[3];
2544         calibrator.getInitialBias(bias2);
2545         assertArrayEquals(bias1, bias2, 0.0);
2546         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2547         assertEquals(b1, ba);
2548         final Matrix b2 = new Matrix(3, 1);
2549         calibrator.getInitialBiasAsMatrix(b2);
2550         assertEquals(b1, b2);
2551         final Matrix ma1 = calibrator.getInitialMa();
2552         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2553         final Matrix ma2 = new Matrix(3, 3);
2554         calibrator.getInitialMa(ma2);
2555         assertEquals(ma1, ma2);
2556         assertNull(calibrator.getMeasurements());
2557         assertFalse(calibrator.isCommonAxisUsed());
2558         assertNull(calibrator.getListener());
2559         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2560         assertFalse(calibrator.isReady());
2561         assertFalse(calibrator.isRunning());
2562         assertNull(calibrator.getEstimatedBiases());
2563         assertFalse(calibrator.getEstimatedBiases(null));
2564         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2565         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2566         assertNull(calibrator.getEstimatedBiasFx());
2567         assertNull(calibrator.getEstimatedBiasFy());
2568         assertNull(calibrator.getEstimatedBiasFz());
2569         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2570         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2571         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2572         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2573         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2574         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2575         assertNull(calibrator.getEstimatedMa());
2576         assertNull(calibrator.getEstimatedSx());
2577         assertNull(calibrator.getEstimatedSy());
2578         assertNull(calibrator.getEstimatedSz());
2579         assertNull(calibrator.getEstimatedMxy());
2580         assertNull(calibrator.getEstimatedMxz());
2581         assertNull(calibrator.getEstimatedMyx());
2582         assertNull(calibrator.getEstimatedMyz());
2583         assertNull(calibrator.getEstimatedMzx());
2584         assertNull(calibrator.getEstimatedMzy());
2585         assertNull(calibrator.getEstimatedCovariance());
2586         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2587         assertNull(calibrator.getGroundTruthGravityNorm());
2588         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2589         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2590     }
2591 
2592     @Test
2593     public void testConstructor26() throws WrongSizeException {
2594         final Collection<StandardDeviationBodyKinematics> measurements =
2595                 Collections.emptyList();
2596 
2597         final Matrix ba = generateBa();
2598         final double biasX = ba.getElementAtIndex(0);
2599         final double biasY = ba.getElementAtIndex(1);
2600         final double biasZ = ba.getElementAtIndex(2);
2601 
2602         final Matrix ma = generateMaCommonAxis();
2603         final double sx = ma.getElementAt(0, 0);
2604         final double sy = ma.getElementAt(1, 1);
2605         final double sz = ma.getElementAt(2, 2);
2606 
2607         final KnownGravityNormAccelerometerCalibrator calibrator =
2608                 new KnownGravityNormAccelerometerCalibrator(measurements,
2609                         biasX, biasY, biasZ, sx, sy, sz);
2610 
2611         // check default values
2612         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2613         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2614         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2615         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2616         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2617         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2618         final Acceleration bx2 = new Acceleration(0.0,
2619                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2620         calibrator.getInitialBiasXAsAcceleration(bx2);
2621         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2622         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2623         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2624         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2625         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2626         final Acceleration by2 = new Acceleration(0.0,
2627                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2628         calibrator.getInitialBiasYAsAcceleration(by2);
2629         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2630         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2631         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2632         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2633         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2634         final Acceleration bz2 = new Acceleration(0.0,
2635                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2636         calibrator.getInitialBiasZAsAcceleration(bz2);
2637         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2638         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2639         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2640         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2641         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2642         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2643         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2644         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2645         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2646         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2647         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2648         final double[] bias1 = calibrator.getInitialBias();
2649         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2650         final double[] bias2 = new double[3];
2651         calibrator.getInitialBias(bias2);
2652         assertArrayEquals(bias1, bias2, 0.0);
2653         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2654         assertEquals(b1, ba);
2655         final Matrix b2 = new Matrix(3, 1);
2656         calibrator.getInitialBiasAsMatrix(b2);
2657         assertEquals(b1, b2);
2658         final Matrix ma1 = calibrator.getInitialMa();
2659         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2660         final Matrix ma2 = new Matrix(3, 3);
2661         calibrator.getInitialMa(ma2);
2662         assertEquals(ma1, ma2);
2663         assertSame(calibrator.getMeasurements(), measurements);
2664         assertFalse(calibrator.isCommonAxisUsed());
2665         assertNull(calibrator.getListener());
2666         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2667         assertFalse(calibrator.isReady());
2668         assertFalse(calibrator.isRunning());
2669         assertNull(calibrator.getEstimatedBiases());
2670         assertFalse(calibrator.getEstimatedBiases(null));
2671         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2672         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2673         assertNull(calibrator.getEstimatedBiasFx());
2674         assertNull(calibrator.getEstimatedBiasFy());
2675         assertNull(calibrator.getEstimatedBiasFz());
2676         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2677         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2678         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2679         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2680         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2681         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2682         assertNull(calibrator.getEstimatedMa());
2683         assertNull(calibrator.getEstimatedSx());
2684         assertNull(calibrator.getEstimatedSy());
2685         assertNull(calibrator.getEstimatedSz());
2686         assertNull(calibrator.getEstimatedMxy());
2687         assertNull(calibrator.getEstimatedMxz());
2688         assertNull(calibrator.getEstimatedMyx());
2689         assertNull(calibrator.getEstimatedMyz());
2690         assertNull(calibrator.getEstimatedMzx());
2691         assertNull(calibrator.getEstimatedMzy());
2692         assertNull(calibrator.getEstimatedCovariance());
2693         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2694         assertNull(calibrator.getGroundTruthGravityNorm());
2695         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2696         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2697     }
2698 
2699     @Test
2700     public void testConstructor27() throws WrongSizeException {
2701         final Collection<StandardDeviationBodyKinematics> measurements =
2702                 Collections.emptyList();
2703 
2704         final Matrix ba = generateBa();
2705         final double biasX = ba.getElementAtIndex(0);
2706         final double biasY = ba.getElementAtIndex(1);
2707         final double biasZ = ba.getElementAtIndex(2);
2708 
2709         final Matrix ma = generateMaCommonAxis();
2710         final double sx = ma.getElementAt(0, 0);
2711         final double sy = ma.getElementAt(1, 1);
2712         final double sz = ma.getElementAt(2, 2);
2713 
2714         final KnownGravityNormAccelerometerCalibrator calibrator =
2715                 new KnownGravityNormAccelerometerCalibrator(measurements,
2716                         biasX, biasY, biasZ, sx, sy, sz, this);
2717 
2718         // check default values
2719         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2720         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2721         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2722         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2723         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2724         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2725         final Acceleration bx2 = new Acceleration(0.0,
2726                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2727         calibrator.getInitialBiasXAsAcceleration(bx2);
2728         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2729         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2730         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2731         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2732         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2733         final Acceleration by2 = new Acceleration(0.0,
2734                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2735         calibrator.getInitialBiasYAsAcceleration(by2);
2736         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2737         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2738         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2739         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2740         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2741         final Acceleration bz2 = new Acceleration(0.0,
2742                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2743         calibrator.getInitialBiasZAsAcceleration(bz2);
2744         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2745         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2746         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2747         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2748         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2749         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2750         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2751         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2752         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2753         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2754         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2755         final double[] bias1 = calibrator.getInitialBias();
2756         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2757         final double[] bias2 = new double[3];
2758         calibrator.getInitialBias(bias2);
2759         assertArrayEquals(bias1, bias2, 0.0);
2760         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2761         assertEquals(b1, ba);
2762         final Matrix b2 = new Matrix(3, 1);
2763         calibrator.getInitialBiasAsMatrix(b2);
2764         assertEquals(b1, b2);
2765         final Matrix ma1 = calibrator.getInitialMa();
2766         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2767         final Matrix ma2 = new Matrix(3, 3);
2768         calibrator.getInitialMa(ma2);
2769         assertEquals(ma1, ma2);
2770         assertSame(calibrator.getMeasurements(), measurements);
2771         assertFalse(calibrator.isCommonAxisUsed());
2772         assertSame(calibrator.getListener(), this);
2773         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2774         assertFalse(calibrator.isReady());
2775         assertFalse(calibrator.isRunning());
2776         assertNull(calibrator.getEstimatedBiases());
2777         assertFalse(calibrator.getEstimatedBiases(null));
2778         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2779         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2780         assertNull(calibrator.getEstimatedBiasFx());
2781         assertNull(calibrator.getEstimatedBiasFy());
2782         assertNull(calibrator.getEstimatedBiasFz());
2783         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2784         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2785         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2786         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2787         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2788         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2789         assertNull(calibrator.getEstimatedMa());
2790         assertNull(calibrator.getEstimatedSx());
2791         assertNull(calibrator.getEstimatedSy());
2792         assertNull(calibrator.getEstimatedSz());
2793         assertNull(calibrator.getEstimatedMxy());
2794         assertNull(calibrator.getEstimatedMxz());
2795         assertNull(calibrator.getEstimatedMyx());
2796         assertNull(calibrator.getEstimatedMyz());
2797         assertNull(calibrator.getEstimatedMzx());
2798         assertNull(calibrator.getEstimatedMzy());
2799         assertNull(calibrator.getEstimatedCovariance());
2800         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2801         assertNull(calibrator.getGroundTruthGravityNorm());
2802         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2803         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2804     }
2805 
2806     @Test
2807     public void testConstructor28() throws WrongSizeException {
2808         final Matrix ba = generateBa();
2809         final double biasX = ba.getElementAtIndex(0);
2810         final double biasY = ba.getElementAtIndex(1);
2811         final double biasZ = ba.getElementAtIndex(2);
2812 
2813         final Matrix ma = generateMaCommonAxis();
2814         final double sx = ma.getElementAt(0, 0);
2815         final double sy = ma.getElementAt(1, 1);
2816         final double sz = ma.getElementAt(2, 2);
2817 
2818         final KnownGravityNormAccelerometerCalibrator calibrator =
2819                 new KnownGravityNormAccelerometerCalibrator(
2820                         true, biasX, biasY, biasZ, sx, sy, sz);
2821 
2822         // check default values
2823         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2824         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2825         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2826         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2827         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2828         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2829         final Acceleration bx2 = new Acceleration(0.0,
2830                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2831         calibrator.getInitialBiasXAsAcceleration(bx2);
2832         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2833         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2834         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2835         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2836         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2837         final Acceleration by2 = new Acceleration(0.0,
2838                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2839         calibrator.getInitialBiasYAsAcceleration(by2);
2840         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2841         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2842         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2843         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2844         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2845         final Acceleration bz2 = new Acceleration(0.0,
2846                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2847         calibrator.getInitialBiasZAsAcceleration(bz2);
2848         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2849         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2850         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2851         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2852         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2853         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2854         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2855         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2856         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2857         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2858         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2859         final double[] bias1 = calibrator.getInitialBias();
2860         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2861         final double[] bias2 = new double[3];
2862         calibrator.getInitialBias(bias2);
2863         assertArrayEquals(bias1, bias2, 0.0);
2864         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2865         assertEquals(b1, ba);
2866         final Matrix b2 = new Matrix(3, 1);
2867         calibrator.getInitialBiasAsMatrix(b2);
2868         assertEquals(b1, b2);
2869         final Matrix ma1 = calibrator.getInitialMa();
2870         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2871         final Matrix ma2 = new Matrix(3, 3);
2872         calibrator.getInitialMa(ma2);
2873         assertEquals(ma1, ma2);
2874         assertNull(calibrator.getMeasurements());
2875         assertTrue(calibrator.isCommonAxisUsed());
2876         assertNull(calibrator.getListener());
2877         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2878         assertFalse(calibrator.isReady());
2879         assertFalse(calibrator.isRunning());
2880         assertNull(calibrator.getEstimatedBiases());
2881         assertFalse(calibrator.getEstimatedBiases(null));
2882         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2883         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2884         assertNull(calibrator.getEstimatedBiasFx());
2885         assertNull(calibrator.getEstimatedBiasFy());
2886         assertNull(calibrator.getEstimatedBiasFz());
2887         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2888         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2889         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2890         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2891         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2892         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2893         assertNull(calibrator.getEstimatedMa());
2894         assertNull(calibrator.getEstimatedSx());
2895         assertNull(calibrator.getEstimatedSy());
2896         assertNull(calibrator.getEstimatedSz());
2897         assertNull(calibrator.getEstimatedMxy());
2898         assertNull(calibrator.getEstimatedMxz());
2899         assertNull(calibrator.getEstimatedMyx());
2900         assertNull(calibrator.getEstimatedMyz());
2901         assertNull(calibrator.getEstimatedMzx());
2902         assertNull(calibrator.getEstimatedMzy());
2903         assertNull(calibrator.getEstimatedCovariance());
2904         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2905         assertNull(calibrator.getGroundTruthGravityNorm());
2906         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2907         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2908     }
2909 
2910     @Test
2911     public void testConstructor29() throws WrongSizeException {
2912         final Matrix ba = generateBa();
2913         final double biasX = ba.getElementAtIndex(0);
2914         final double biasY = ba.getElementAtIndex(1);
2915         final double biasZ = ba.getElementAtIndex(2);
2916 
2917         final Matrix ma = generateMaCommonAxis();
2918         final double sx = ma.getElementAt(0, 0);
2919         final double sy = ma.getElementAt(1, 1);
2920         final double sz = ma.getElementAt(2, 2);
2921 
2922         final KnownGravityNormAccelerometerCalibrator calibrator =
2923                 new KnownGravityNormAccelerometerCalibrator(
2924                         true, biasX, biasY, biasZ,
2925                         sx, sy, sz, this);
2926 
2927         // check default values
2928         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2929         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2930         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2931         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2932         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2933         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2934         final Acceleration bx2 = new Acceleration(0.0,
2935                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2936         calibrator.getInitialBiasXAsAcceleration(bx2);
2937         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2938         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2939         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2940         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2941         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2942         final Acceleration by2 = new Acceleration(0.0,
2943                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2944         calibrator.getInitialBiasYAsAcceleration(by2);
2945         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2946         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2947         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2948         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2949         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2950         final Acceleration bz2 = new Acceleration(0.0,
2951                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2952         calibrator.getInitialBiasZAsAcceleration(bz2);
2953         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2954         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2955         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2956         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2957         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2958         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2959         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2960         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2961         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2962         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2963         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2964         final double[] bias1 = calibrator.getInitialBias();
2965         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2966         final double[] bias2 = new double[3];
2967         calibrator.getInitialBias(bias2);
2968         assertArrayEquals(bias1, bias2, 0.0);
2969         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2970         assertEquals(b1, ba);
2971         final Matrix b2 = new Matrix(3, 1);
2972         calibrator.getInitialBiasAsMatrix(b2);
2973         assertEquals(b1, b2);
2974         final Matrix ma1 = calibrator.getInitialMa();
2975         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2976         final Matrix ma2 = new Matrix(3, 3);
2977         calibrator.getInitialMa(ma2);
2978         assertEquals(ma1, ma2);
2979         assertNull(calibrator.getMeasurements());
2980         assertTrue(calibrator.isCommonAxisUsed());
2981         assertSame(calibrator.getListener(), this);
2982         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2983         assertFalse(calibrator.isReady());
2984         assertFalse(calibrator.isRunning());
2985         assertNull(calibrator.getEstimatedBiases());
2986         assertFalse(calibrator.getEstimatedBiases(null));
2987         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2988         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2989         assertNull(calibrator.getEstimatedBiasFx());
2990         assertNull(calibrator.getEstimatedBiasFy());
2991         assertNull(calibrator.getEstimatedBiasFz());
2992         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2993         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2994         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2995         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2996         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2997         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2998         assertNull(calibrator.getEstimatedMa());
2999         assertNull(calibrator.getEstimatedSx());
3000         assertNull(calibrator.getEstimatedSy());
3001         assertNull(calibrator.getEstimatedSz());
3002         assertNull(calibrator.getEstimatedMxy());
3003         assertNull(calibrator.getEstimatedMxz());
3004         assertNull(calibrator.getEstimatedMyx());
3005         assertNull(calibrator.getEstimatedMyz());
3006         assertNull(calibrator.getEstimatedMzx());
3007         assertNull(calibrator.getEstimatedMzy());
3008         assertNull(calibrator.getEstimatedCovariance());
3009         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3010         assertNull(calibrator.getGroundTruthGravityNorm());
3011         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3012         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3013     }
3014 
3015     @Test
3016     public void testConstructor30() throws WrongSizeException {
3017         final Collection<StandardDeviationBodyKinematics> measurements =
3018                 Collections.emptyList();
3019         final Matrix ba = generateBa();
3020         final double biasX = ba.getElementAtIndex(0);
3021         final double biasY = ba.getElementAtIndex(1);
3022         final double biasZ = ba.getElementAtIndex(2);
3023 
3024         final Matrix ma = generateMaCommonAxis();
3025         final double sx = ma.getElementAt(0, 0);
3026         final double sy = ma.getElementAt(1, 1);
3027         final double sz = ma.getElementAt(2, 2);
3028 
3029         final KnownGravityNormAccelerometerCalibrator calibrator =
3030                 new KnownGravityNormAccelerometerCalibrator(measurements,
3031                         true, biasX, biasY, biasZ,
3032                         sx, sy, sz);
3033 
3034         // check default values
3035         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3036         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3037         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3038         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3039         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3040         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3041         final Acceleration bx2 = new Acceleration(0.0,
3042                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3043         calibrator.getInitialBiasXAsAcceleration(bx2);
3044         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3045         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3046         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3047         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3048         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3049         final Acceleration by2 = new Acceleration(0.0,
3050                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3051         calibrator.getInitialBiasYAsAcceleration(by2);
3052         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3053         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3054         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3055         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3056         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3057         final Acceleration bz2 = new Acceleration(0.0,
3058                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3059         calibrator.getInitialBiasZAsAcceleration(bz2);
3060         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3061         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3062         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3063         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3064         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3065         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3066         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3067         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3068         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3069         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3070         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3071         final double[] bias1 = calibrator.getInitialBias();
3072         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3073         final double[] bias2 = new double[3];
3074         calibrator.getInitialBias(bias2);
3075         assertArrayEquals(bias1, bias2, 0.0);
3076         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3077         assertEquals(b1, ba);
3078         final Matrix b2 = new Matrix(3, 1);
3079         calibrator.getInitialBiasAsMatrix(b2);
3080         assertEquals(b1, b2);
3081         final Matrix ma1 = calibrator.getInitialMa();
3082         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3083         final Matrix ma2 = new Matrix(3, 3);
3084         calibrator.getInitialMa(ma2);
3085         assertEquals(ma1, ma2);
3086         assertSame(calibrator.getMeasurements(), measurements);
3087         assertTrue(calibrator.isCommonAxisUsed());
3088         assertNull(calibrator.getListener());
3089         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3090         assertFalse(calibrator.isReady());
3091         assertFalse(calibrator.isRunning());
3092         assertNull(calibrator.getEstimatedBiases());
3093         assertFalse(calibrator.getEstimatedBiases(null));
3094         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3095         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3096         assertNull(calibrator.getEstimatedBiasFx());
3097         assertNull(calibrator.getEstimatedBiasFy());
3098         assertNull(calibrator.getEstimatedBiasFz());
3099         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3100         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3101         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3102         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3103         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3104         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3105         assertNull(calibrator.getEstimatedMa());
3106         assertNull(calibrator.getEstimatedSx());
3107         assertNull(calibrator.getEstimatedSy());
3108         assertNull(calibrator.getEstimatedSz());
3109         assertNull(calibrator.getEstimatedMxy());
3110         assertNull(calibrator.getEstimatedMxz());
3111         assertNull(calibrator.getEstimatedMyx());
3112         assertNull(calibrator.getEstimatedMyz());
3113         assertNull(calibrator.getEstimatedMzx());
3114         assertNull(calibrator.getEstimatedMzy());
3115         assertNull(calibrator.getEstimatedCovariance());
3116         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3117         assertNull(calibrator.getGroundTruthGravityNorm());
3118         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3119         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3120     }
3121 
3122     @Test
3123     public void testConstructor31() throws WrongSizeException {
3124         final Collection<StandardDeviationBodyKinematics> measurements =
3125                 Collections.emptyList();
3126         final Matrix ba = generateBa();
3127         final double biasX = ba.getElementAtIndex(0);
3128         final double biasY = ba.getElementAtIndex(1);
3129         final double biasZ = ba.getElementAtIndex(2);
3130 
3131         final Matrix ma = generateMaCommonAxis();
3132         final double sx = ma.getElementAt(0, 0);
3133         final double sy = ma.getElementAt(1, 1);
3134         final double sz = ma.getElementAt(2, 2);
3135 
3136         final KnownGravityNormAccelerometerCalibrator calibrator =
3137                 new KnownGravityNormAccelerometerCalibrator(measurements,
3138                         true, biasX, biasY, biasZ,
3139                         sx, sy, sz, this);
3140 
3141         // check default values
3142         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3143         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3144         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3145         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3146         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3147         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3148         final Acceleration bx2 = new Acceleration(0.0,
3149                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3150         calibrator.getInitialBiasXAsAcceleration(bx2);
3151         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3152         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3153         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3154         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3155         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3156         final Acceleration by2 = new Acceleration(0.0,
3157                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3158         calibrator.getInitialBiasYAsAcceleration(by2);
3159         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3160         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3161         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3162         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3163         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3164         final Acceleration bz2 = new Acceleration(0.0,
3165                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3166         calibrator.getInitialBiasZAsAcceleration(bz2);
3167         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3168         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3169         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3170         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3171         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3172         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3173         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3174         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3175         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3176         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3177         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3178         final double[] bias1 = calibrator.getInitialBias();
3179         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3180         final double[] bias2 = new double[3];
3181         calibrator.getInitialBias(bias2);
3182         assertArrayEquals(bias1, bias2, 0.0);
3183         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3184         assertEquals(b1, ba);
3185         final Matrix b2 = new Matrix(3, 1);
3186         calibrator.getInitialBiasAsMatrix(b2);
3187         assertEquals(b1, b2);
3188         final Matrix ma1 = calibrator.getInitialMa();
3189         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3190         final Matrix ma2 = new Matrix(3, 3);
3191         calibrator.getInitialMa(ma2);
3192         assertEquals(ma1, ma2);
3193         assertSame(calibrator.getMeasurements(), measurements);
3194         assertTrue(calibrator.isCommonAxisUsed());
3195         assertSame(calibrator.getListener(), this);
3196         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3197         assertFalse(calibrator.isReady());
3198         assertFalse(calibrator.isRunning());
3199         assertNull(calibrator.getEstimatedBiases());
3200         assertFalse(calibrator.getEstimatedBiases(null));
3201         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3202         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3203         assertNull(calibrator.getEstimatedBiasFx());
3204         assertNull(calibrator.getEstimatedBiasFy());
3205         assertNull(calibrator.getEstimatedBiasFz());
3206         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3207         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3208         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3209         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3210         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3211         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3212         assertNull(calibrator.getEstimatedMa());
3213         assertNull(calibrator.getEstimatedSx());
3214         assertNull(calibrator.getEstimatedSy());
3215         assertNull(calibrator.getEstimatedSz());
3216         assertNull(calibrator.getEstimatedMxy());
3217         assertNull(calibrator.getEstimatedMxz());
3218         assertNull(calibrator.getEstimatedMyx());
3219         assertNull(calibrator.getEstimatedMyz());
3220         assertNull(calibrator.getEstimatedMzx());
3221         assertNull(calibrator.getEstimatedMzy());
3222         assertNull(calibrator.getEstimatedCovariance());
3223         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3224         assertNull(calibrator.getGroundTruthGravityNorm());
3225         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3226         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3227     }
3228 
3229     @Test
3230     public void testConstructor32() throws WrongSizeException {
3231         final Matrix ba = generateBa();
3232         final double biasX = ba.getElementAtIndex(0);
3233         final double biasY = ba.getElementAtIndex(1);
3234         final double biasZ = ba.getElementAtIndex(2);
3235 
3236         final Matrix ma = generateMaCommonAxis();
3237         final double sx = ma.getElementAt(0, 0);
3238         final double sy = ma.getElementAt(1, 1);
3239         final double sz = ma.getElementAt(2, 2);
3240 
3241         final Acceleration bx = new Acceleration(biasX,
3242                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3243         final Acceleration by = new Acceleration(biasY,
3244                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3245         final Acceleration bz = new Acceleration(biasZ,
3246                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3247 
3248         final KnownGravityNormAccelerometerCalibrator calibrator =
3249                 new KnownGravityNormAccelerometerCalibrator(bx, by, bz,
3250                         sx, sy, sz);
3251 
3252         // check default values
3253         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3254         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3255         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3256         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3257         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3258         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3259         final Acceleration bx2 = new Acceleration(0.0,
3260                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3261         calibrator.getInitialBiasXAsAcceleration(bx2);
3262         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3263         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3264         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3265         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3266         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3267         final Acceleration by2 = new Acceleration(0.0,
3268                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3269         calibrator.getInitialBiasYAsAcceleration(by2);
3270         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3271         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3272         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3273         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3274         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3275         final Acceleration bz2 = new Acceleration(0.0,
3276                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3277         calibrator.getInitialBiasZAsAcceleration(bz2);
3278         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3279         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3280         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3281         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3282         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3283         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3284         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3285         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3286         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3287         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3288         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3289         final double[] bias1 = calibrator.getInitialBias();
3290         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3291         final double[] bias2 = new double[3];
3292         calibrator.getInitialBias(bias2);
3293         assertArrayEquals(bias1, bias2, 0.0);
3294         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3295         assertEquals(b1, ba);
3296         final Matrix b2 = new Matrix(3, 1);
3297         calibrator.getInitialBiasAsMatrix(b2);
3298         assertEquals(b1, b2);
3299         final Matrix ma1 = calibrator.getInitialMa();
3300         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3301         final Matrix ma2 = new Matrix(3, 3);
3302         calibrator.getInitialMa(ma2);
3303         assertEquals(ma1, ma2);
3304         assertNull(calibrator.getMeasurements());
3305         assertFalse(calibrator.isCommonAxisUsed());
3306         assertNull(calibrator.getListener());
3307         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3308         assertFalse(calibrator.isReady());
3309         assertFalse(calibrator.isRunning());
3310         assertNull(calibrator.getEstimatedBiases());
3311         assertFalse(calibrator.getEstimatedBiases(null));
3312         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3313         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3314         assertNull(calibrator.getEstimatedBiasFx());
3315         assertNull(calibrator.getEstimatedBiasFy());
3316         assertNull(calibrator.getEstimatedBiasFz());
3317         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3318         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3319         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3320         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3321         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3322         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3323         assertNull(calibrator.getEstimatedMa());
3324         assertNull(calibrator.getEstimatedSx());
3325         assertNull(calibrator.getEstimatedSy());
3326         assertNull(calibrator.getEstimatedSz());
3327         assertNull(calibrator.getEstimatedMxy());
3328         assertNull(calibrator.getEstimatedMxz());
3329         assertNull(calibrator.getEstimatedMyx());
3330         assertNull(calibrator.getEstimatedMyz());
3331         assertNull(calibrator.getEstimatedMzx());
3332         assertNull(calibrator.getEstimatedMzy());
3333         assertNull(calibrator.getEstimatedCovariance());
3334         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3335         assertNull(calibrator.getGroundTruthGravityNorm());
3336         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3337         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3338     }
3339 
3340     @Test
3341     public void testConstructor33() throws WrongSizeException {
3342         final Matrix ba = generateBa();
3343         final double biasX = ba.getElementAtIndex(0);
3344         final double biasY = ba.getElementAtIndex(1);
3345         final double biasZ = ba.getElementAtIndex(2);
3346 
3347         final Matrix ma = generateMaCommonAxis();
3348         final double sx = ma.getElementAt(0, 0);
3349         final double sy = ma.getElementAt(1, 1);
3350         final double sz = ma.getElementAt(2, 2);
3351 
3352         final Acceleration bx = new Acceleration(biasX,
3353                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3354         final Acceleration by = new Acceleration(biasY,
3355                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3356         final Acceleration bz = new Acceleration(biasZ,
3357                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3358 
3359         final KnownGravityNormAccelerometerCalibrator calibrator =
3360                 new KnownGravityNormAccelerometerCalibrator(bx, by, bz,
3361                         sx, sy, sz, this);
3362 
3363         // check default values
3364         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3365         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3366         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3367         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3368         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3369         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3370         final Acceleration bx2 = new Acceleration(0.0,
3371                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3372         calibrator.getInitialBiasXAsAcceleration(bx2);
3373         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3374         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3375         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3376         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3377         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3378         final Acceleration by2 = new Acceleration(0.0,
3379                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3380         calibrator.getInitialBiasYAsAcceleration(by2);
3381         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3382         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3383         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3384         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3385         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3386         final Acceleration bz2 = new Acceleration(0.0,
3387                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3388         calibrator.getInitialBiasZAsAcceleration(bz2);
3389         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3390         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3391         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3392         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3393         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3394         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3395         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3396         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3397         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3398         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3399         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3400         final double[] bias1 = calibrator.getInitialBias();
3401         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3402         final double[] bias2 = new double[3];
3403         calibrator.getInitialBias(bias2);
3404         assertArrayEquals(bias1, bias2, 0.0);
3405         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3406         assertEquals(b1, ba);
3407         final Matrix b2 = new Matrix(3, 1);
3408         calibrator.getInitialBiasAsMatrix(b2);
3409         assertEquals(b1, b2);
3410         final Matrix ma1 = calibrator.getInitialMa();
3411         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3412         final Matrix ma2 = new Matrix(3, 3);
3413         calibrator.getInitialMa(ma2);
3414         assertEquals(ma1, ma2);
3415         assertNull(calibrator.getMeasurements());
3416         assertFalse(calibrator.isCommonAxisUsed());
3417         assertSame(calibrator.getListener(), this);
3418         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3419         assertFalse(calibrator.isReady());
3420         assertFalse(calibrator.isRunning());
3421         assertNull(calibrator.getEstimatedBiases());
3422         assertFalse(calibrator.getEstimatedBiases(null));
3423         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3424         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3425         assertNull(calibrator.getEstimatedBiasFx());
3426         assertNull(calibrator.getEstimatedBiasFy());
3427         assertNull(calibrator.getEstimatedBiasFz());
3428         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3429         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3430         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3431         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3432         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3433         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3434         assertNull(calibrator.getEstimatedMa());
3435         assertNull(calibrator.getEstimatedSx());
3436         assertNull(calibrator.getEstimatedSy());
3437         assertNull(calibrator.getEstimatedSz());
3438         assertNull(calibrator.getEstimatedMxy());
3439         assertNull(calibrator.getEstimatedMxz());
3440         assertNull(calibrator.getEstimatedMyx());
3441         assertNull(calibrator.getEstimatedMyz());
3442         assertNull(calibrator.getEstimatedMzx());
3443         assertNull(calibrator.getEstimatedMzy());
3444         assertNull(calibrator.getEstimatedCovariance());
3445         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3446         assertNull(calibrator.getGroundTruthGravityNorm());
3447         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3448         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3449     }
3450 
3451     @Test
3452     public void testConstructor34() throws WrongSizeException {
3453         final Collection<StandardDeviationBodyKinematics> measurements =
3454                 Collections.emptyList();
3455 
3456         final Matrix ba = generateBa();
3457         final double biasX = ba.getElementAtIndex(0);
3458         final double biasY = ba.getElementAtIndex(1);
3459         final double biasZ = ba.getElementAtIndex(2);
3460 
3461         final Matrix ma = generateMaCommonAxis();
3462         final double sx = ma.getElementAt(0, 0);
3463         final double sy = ma.getElementAt(1, 1);
3464         final double sz = ma.getElementAt(2, 2);
3465 
3466         final Acceleration bx = new Acceleration(biasX,
3467                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3468         final Acceleration by = new Acceleration(biasY,
3469                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3470         final Acceleration bz = new Acceleration(biasZ,
3471                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3472 
3473         final KnownGravityNormAccelerometerCalibrator calibrator =
3474                 new KnownGravityNormAccelerometerCalibrator(measurements,
3475                         bx, by, bz, sx, sy, sz);
3476 
3477         // check default values
3478         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3479         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3480         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3481         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3482         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3483         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3484         final Acceleration bx2 = new Acceleration(0.0,
3485                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3486         calibrator.getInitialBiasXAsAcceleration(bx2);
3487         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3488         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3489         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3490         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3491         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3492         final Acceleration by2 = new Acceleration(0.0,
3493                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3494         calibrator.getInitialBiasYAsAcceleration(by2);
3495         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3496         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3497         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3498         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3499         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3500         final Acceleration bz2 = new Acceleration(0.0,
3501                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3502         calibrator.getInitialBiasZAsAcceleration(bz2);
3503         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3504         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3505         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3506         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3507         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3508         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3509         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3510         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3511         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3512         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3513         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3514         final double[] bias1 = calibrator.getInitialBias();
3515         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3516         final double[] bias2 = new double[3];
3517         calibrator.getInitialBias(bias2);
3518         assertArrayEquals(bias1, bias2, 0.0);
3519         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3520         assertEquals(b1, ba);
3521         final Matrix b2 = new Matrix(3, 1);
3522         calibrator.getInitialBiasAsMatrix(b2);
3523         assertEquals(b1, b2);
3524         final Matrix ma1 = calibrator.getInitialMa();
3525         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3526         final Matrix ma2 = new Matrix(3, 3);
3527         calibrator.getInitialMa(ma2);
3528         assertEquals(ma1, ma2);
3529         assertSame(calibrator.getMeasurements(), measurements);
3530         assertFalse(calibrator.isCommonAxisUsed());
3531         assertNull(calibrator.getListener());
3532         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3533         assertFalse(calibrator.isReady());
3534         assertFalse(calibrator.isRunning());
3535         assertNull(calibrator.getEstimatedBiases());
3536         assertFalse(calibrator.getEstimatedBiases(null));
3537         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3538         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3539         assertNull(calibrator.getEstimatedBiasFx());
3540         assertNull(calibrator.getEstimatedBiasFy());
3541         assertNull(calibrator.getEstimatedBiasFz());
3542         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3543         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3544         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3545         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3546         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3547         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3548         assertNull(calibrator.getEstimatedMa());
3549         assertNull(calibrator.getEstimatedSx());
3550         assertNull(calibrator.getEstimatedSy());
3551         assertNull(calibrator.getEstimatedSz());
3552         assertNull(calibrator.getEstimatedMxy());
3553         assertNull(calibrator.getEstimatedMxz());
3554         assertNull(calibrator.getEstimatedMyx());
3555         assertNull(calibrator.getEstimatedMyz());
3556         assertNull(calibrator.getEstimatedMzx());
3557         assertNull(calibrator.getEstimatedMzy());
3558         assertNull(calibrator.getEstimatedCovariance());
3559         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3560         assertNull(calibrator.getGroundTruthGravityNorm());
3561         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3562         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3563     }
3564 
3565     @Test
3566     public void testConstructor35() throws WrongSizeException {
3567         final Collection<StandardDeviationBodyKinematics> measurements =
3568                 Collections.emptyList();
3569 
3570         final Matrix ba = generateBa();
3571         final double biasX = ba.getElementAtIndex(0);
3572         final double biasY = ba.getElementAtIndex(1);
3573         final double biasZ = ba.getElementAtIndex(2);
3574 
3575         final Matrix ma = generateMaCommonAxis();
3576         final double sx = ma.getElementAt(0, 0);
3577         final double sy = ma.getElementAt(1, 1);
3578         final double sz = ma.getElementAt(2, 2);
3579 
3580         final Acceleration bx = new Acceleration(biasX,
3581                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3582         final Acceleration by = new Acceleration(biasY,
3583                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3584         final Acceleration bz = new Acceleration(biasZ,
3585                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3586 
3587         final KnownGravityNormAccelerometerCalibrator calibrator =
3588                 new KnownGravityNormAccelerometerCalibrator(measurements,
3589                         bx, by, bz, sx, sy, sz, this);
3590 
3591         // check default values
3592         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3593         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3594         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3595         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3596         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3597         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3598         final Acceleration bx2 = new Acceleration(0.0,
3599                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3600         calibrator.getInitialBiasXAsAcceleration(bx2);
3601         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3602         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3603         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3604         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3605         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3606         final Acceleration by2 = new Acceleration(0.0,
3607                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3608         calibrator.getInitialBiasYAsAcceleration(by2);
3609         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3610         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3611         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3612         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3613         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3614         final Acceleration bz2 = new Acceleration(0.0,
3615                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3616         calibrator.getInitialBiasZAsAcceleration(bz2);
3617         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3618         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3619         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3620         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3621         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3622         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3623         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3624         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3625         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3626         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3627         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3628         final double[] bias1 = calibrator.getInitialBias();
3629         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3630         final double[] bias2 = new double[3];
3631         calibrator.getInitialBias(bias2);
3632         assertArrayEquals(bias1, bias2, 0.0);
3633         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3634         assertEquals(b1, ba);
3635         final Matrix b2 = new Matrix(3, 1);
3636         calibrator.getInitialBiasAsMatrix(b2);
3637         assertEquals(b1, b2);
3638         final Matrix ma1 = calibrator.getInitialMa();
3639         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3640         final Matrix ma2 = new Matrix(3, 3);
3641         calibrator.getInitialMa(ma2);
3642         assertEquals(ma1, ma2);
3643         assertSame(calibrator.getMeasurements(), measurements);
3644         assertFalse(calibrator.isCommonAxisUsed());
3645         assertSame(calibrator.getListener(), this);
3646         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3647         assertFalse(calibrator.isReady());
3648         assertFalse(calibrator.isRunning());
3649         assertNull(calibrator.getEstimatedBiases());
3650         assertFalse(calibrator.getEstimatedBiases(null));
3651         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3652         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3653         assertNull(calibrator.getEstimatedBiasFx());
3654         assertNull(calibrator.getEstimatedBiasFy());
3655         assertNull(calibrator.getEstimatedBiasFz());
3656         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3657         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3658         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3659         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3660         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3661         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3662         assertNull(calibrator.getEstimatedMa());
3663         assertNull(calibrator.getEstimatedSx());
3664         assertNull(calibrator.getEstimatedSy());
3665         assertNull(calibrator.getEstimatedSz());
3666         assertNull(calibrator.getEstimatedMxy());
3667         assertNull(calibrator.getEstimatedMxz());
3668         assertNull(calibrator.getEstimatedMyx());
3669         assertNull(calibrator.getEstimatedMyz());
3670         assertNull(calibrator.getEstimatedMzx());
3671         assertNull(calibrator.getEstimatedMzy());
3672         assertNull(calibrator.getEstimatedCovariance());
3673         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3674         assertNull(calibrator.getGroundTruthGravityNorm());
3675         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3676         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3677     }
3678 
3679     @Test
3680     public void testConstructor36() throws WrongSizeException {
3681         final Matrix ba = generateBa();
3682         final double biasX = ba.getElementAtIndex(0);
3683         final double biasY = ba.getElementAtIndex(1);
3684         final double biasZ = ba.getElementAtIndex(2);
3685 
3686         final Matrix ma = generateMaCommonAxis();
3687         final double sx = ma.getElementAt(0, 0);
3688         final double sy = ma.getElementAt(1, 1);
3689         final double sz = ma.getElementAt(2, 2);
3690 
3691         final Acceleration bx = new Acceleration(biasX,
3692                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3693         final Acceleration by = new Acceleration(biasY,
3694                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3695         final Acceleration bz = new Acceleration(biasZ,
3696                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3697 
3698         final KnownGravityNormAccelerometerCalibrator calibrator =
3699                 new KnownGravityNormAccelerometerCalibrator(true,
3700                         bx, by, bz, sx, sy, sz);
3701 
3702         // check default values
3703         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3704         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3705         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3706         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3707         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3708         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3709         final Acceleration bx2 = new Acceleration(0.0,
3710                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3711         calibrator.getInitialBiasXAsAcceleration(bx2);
3712         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3713         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3714         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3715         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3716         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3717         final Acceleration by2 = new Acceleration(0.0,
3718                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3719         calibrator.getInitialBiasYAsAcceleration(by2);
3720         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3721         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3722         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3723         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3724         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3725         final Acceleration bz2 = new Acceleration(0.0,
3726                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3727         calibrator.getInitialBiasZAsAcceleration(bz2);
3728         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3729         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3730         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3731         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3732         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3733         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3734         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3735         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3736         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3737         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3738         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3739         final double[] bias1 = calibrator.getInitialBias();
3740         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3741         final double[] bias2 = new double[3];
3742         calibrator.getInitialBias(bias2);
3743         assertArrayEquals(bias1, bias2, 0.0);
3744         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3745         assertEquals(b1, ba);
3746         final Matrix b2 = new Matrix(3, 1);
3747         calibrator.getInitialBiasAsMatrix(b2);
3748         assertEquals(b1, b2);
3749         final Matrix ma1 = calibrator.getInitialMa();
3750         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3751         final Matrix ma2 = new Matrix(3, 3);
3752         calibrator.getInitialMa(ma2);
3753         assertEquals(ma1, ma2);
3754         assertNull(calibrator.getMeasurements());
3755         assertTrue(calibrator.isCommonAxisUsed());
3756         assertNull(calibrator.getListener());
3757         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3758         assertFalse(calibrator.isReady());
3759         assertFalse(calibrator.isRunning());
3760         assertNull(calibrator.getEstimatedBiases());
3761         assertFalse(calibrator.getEstimatedBiases(null));
3762         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3763         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3764         assertNull(calibrator.getEstimatedBiasFx());
3765         assertNull(calibrator.getEstimatedBiasFy());
3766         assertNull(calibrator.getEstimatedBiasFz());
3767         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3768         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3769         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3770         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3771         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3772         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3773         assertNull(calibrator.getEstimatedMa());
3774         assertNull(calibrator.getEstimatedSx());
3775         assertNull(calibrator.getEstimatedSy());
3776         assertNull(calibrator.getEstimatedSz());
3777         assertNull(calibrator.getEstimatedMxy());
3778         assertNull(calibrator.getEstimatedMxz());
3779         assertNull(calibrator.getEstimatedMyx());
3780         assertNull(calibrator.getEstimatedMyz());
3781         assertNull(calibrator.getEstimatedMzx());
3782         assertNull(calibrator.getEstimatedMzy());
3783         assertNull(calibrator.getEstimatedCovariance());
3784         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3785         assertNull(calibrator.getGroundTruthGravityNorm());
3786         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3787         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3788     }
3789 
3790     @Test
3791     public void testConstructor37() throws WrongSizeException {
3792         final Matrix ba = generateBa();
3793         final double biasX = ba.getElementAtIndex(0);
3794         final double biasY = ba.getElementAtIndex(1);
3795         final double biasZ = ba.getElementAtIndex(2);
3796 
3797         final Matrix ma = generateMaCommonAxis();
3798         final double sx = ma.getElementAt(0, 0);
3799         final double sy = ma.getElementAt(1, 1);
3800         final double sz = ma.getElementAt(2, 2);
3801 
3802         final Acceleration bx = new Acceleration(biasX,
3803                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3804         final Acceleration by = new Acceleration(biasY,
3805                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3806         final Acceleration bz = new Acceleration(biasZ,
3807                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3808 
3809         final KnownGravityNormAccelerometerCalibrator calibrator =
3810                 new KnownGravityNormAccelerometerCalibrator(true,
3811                         bx, by, bz, sx, sy, sz, this);
3812 
3813         // check default values
3814         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3815         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3816         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3817         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3818         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3819         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3820         final Acceleration bx2 = new Acceleration(0.0,
3821                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3822         calibrator.getInitialBiasXAsAcceleration(bx2);
3823         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3824         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3825         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3826         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3827         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3828         final Acceleration by2 = new Acceleration(0.0,
3829                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3830         calibrator.getInitialBiasYAsAcceleration(by2);
3831         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3832         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3833         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3834         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3835         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3836         final Acceleration bz2 = new Acceleration(0.0,
3837                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3838         calibrator.getInitialBiasZAsAcceleration(bz2);
3839         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3840         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3841         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3842         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3843         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3844         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3845         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3846         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3847         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3848         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3849         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3850         final double[] bias1 = calibrator.getInitialBias();
3851         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3852         final double[] bias2 = new double[3];
3853         calibrator.getInitialBias(bias2);
3854         assertArrayEquals(bias1, bias2, 0.0);
3855         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3856         assertEquals(b1, ba);
3857         final Matrix b2 = new Matrix(3, 1);
3858         calibrator.getInitialBiasAsMatrix(b2);
3859         assertEquals(b1, b2);
3860         final Matrix ma1 = calibrator.getInitialMa();
3861         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3862         final Matrix ma2 = new Matrix(3, 3);
3863         calibrator.getInitialMa(ma2);
3864         assertEquals(ma1, ma2);
3865         assertNull(calibrator.getMeasurements());
3866         assertTrue(calibrator.isCommonAxisUsed());
3867         assertSame(calibrator.getListener(), this);
3868         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3869         assertFalse(calibrator.isReady());
3870         assertFalse(calibrator.isRunning());
3871         assertNull(calibrator.getEstimatedBiases());
3872         assertFalse(calibrator.getEstimatedBiases(null));
3873         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3874         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3875         assertNull(calibrator.getEstimatedBiasFx());
3876         assertNull(calibrator.getEstimatedBiasFy());
3877         assertNull(calibrator.getEstimatedBiasFz());
3878         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3879         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3880         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3881         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3882         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3883         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3884         assertNull(calibrator.getEstimatedMa());
3885         assertNull(calibrator.getEstimatedSx());
3886         assertNull(calibrator.getEstimatedSy());
3887         assertNull(calibrator.getEstimatedSz());
3888         assertNull(calibrator.getEstimatedMxy());
3889         assertNull(calibrator.getEstimatedMxz());
3890         assertNull(calibrator.getEstimatedMyx());
3891         assertNull(calibrator.getEstimatedMyz());
3892         assertNull(calibrator.getEstimatedMzx());
3893         assertNull(calibrator.getEstimatedMzy());
3894         assertNull(calibrator.getEstimatedCovariance());
3895         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3896         assertNull(calibrator.getGroundTruthGravityNorm());
3897         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3898         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3899     }
3900 
3901     @Test
3902     public void testConstructor38() throws WrongSizeException {
3903         final Collection<StandardDeviationBodyKinematics> measurements =
3904                 Collections.emptyList();
3905 
3906         final Matrix ba = generateBa();
3907         final double biasX = ba.getElementAtIndex(0);
3908         final double biasY = ba.getElementAtIndex(1);
3909         final double biasZ = ba.getElementAtIndex(2);
3910 
3911         final Matrix ma = generateMaCommonAxis();
3912         final double sx = ma.getElementAt(0, 0);
3913         final double sy = ma.getElementAt(1, 1);
3914         final double sz = ma.getElementAt(2, 2);
3915 
3916         final Acceleration bx = new Acceleration(biasX,
3917                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3918         final Acceleration by = new Acceleration(biasY,
3919                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3920         final Acceleration bz = new Acceleration(biasZ,
3921                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3922 
3923         final KnownGravityNormAccelerometerCalibrator calibrator =
3924                 new KnownGravityNormAccelerometerCalibrator(measurements,
3925                         true, bx, by, bz, sx, sy, sz);
3926 
3927         // check default values
3928         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3929         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3930         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3931         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3932         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3933         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3934         final Acceleration bx2 = new Acceleration(0.0,
3935                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3936         calibrator.getInitialBiasXAsAcceleration(bx2);
3937         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3938         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3939         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3940         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3941         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3942         final Acceleration by2 = new Acceleration(0.0,
3943                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3944         calibrator.getInitialBiasYAsAcceleration(by2);
3945         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3946         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3947         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3948         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3949         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3950         final Acceleration bz2 = new Acceleration(0.0,
3951                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3952         calibrator.getInitialBiasZAsAcceleration(bz2);
3953         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3954         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3955         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3956         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3957         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3958         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3959         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3960         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3961         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3962         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3963         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3964         final double[] bias1 = calibrator.getInitialBias();
3965         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3966         final double[] bias2 = new double[3];
3967         calibrator.getInitialBias(bias2);
3968         assertArrayEquals(bias1, bias2, 0.0);
3969         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3970         assertEquals(b1, ba);
3971         final Matrix b2 = new Matrix(3, 1);
3972         calibrator.getInitialBiasAsMatrix(b2);
3973         assertEquals(b1, b2);
3974         final Matrix ma1 = calibrator.getInitialMa();
3975         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3976         final Matrix ma2 = new Matrix(3, 3);
3977         calibrator.getInitialMa(ma2);
3978         assertEquals(ma1, ma2);
3979         assertSame(calibrator.getMeasurements(), measurements);
3980         assertTrue(calibrator.isCommonAxisUsed());
3981         assertNull(calibrator.getListener());
3982         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3983         assertFalse(calibrator.isReady());
3984         assertFalse(calibrator.isRunning());
3985         assertNull(calibrator.getEstimatedBiases());
3986         assertFalse(calibrator.getEstimatedBiases(null));
3987         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3988         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3989         assertNull(calibrator.getEstimatedBiasFx());
3990         assertNull(calibrator.getEstimatedBiasFy());
3991         assertNull(calibrator.getEstimatedBiasFz());
3992         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3993         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3994         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3995         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3996         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3997         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3998         assertNull(calibrator.getEstimatedMa());
3999         assertNull(calibrator.getEstimatedSx());
4000         assertNull(calibrator.getEstimatedSy());
4001         assertNull(calibrator.getEstimatedSz());
4002         assertNull(calibrator.getEstimatedMxy());
4003         assertNull(calibrator.getEstimatedMxz());
4004         assertNull(calibrator.getEstimatedMyx());
4005         assertNull(calibrator.getEstimatedMyz());
4006         assertNull(calibrator.getEstimatedMzx());
4007         assertNull(calibrator.getEstimatedMzy());
4008         assertNull(calibrator.getEstimatedCovariance());
4009         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4010         assertNull(calibrator.getGroundTruthGravityNorm());
4011         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4012         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4013     }
4014 
4015     @Test
4016     public void testConstructor39() throws WrongSizeException {
4017         final Collection<StandardDeviationBodyKinematics> measurements =
4018                 Collections.emptyList();
4019 
4020         final Matrix ba = generateBa();
4021         final double biasX = ba.getElementAtIndex(0);
4022         final double biasY = ba.getElementAtIndex(1);
4023         final double biasZ = ba.getElementAtIndex(2);
4024 
4025         final Matrix ma = generateMaCommonAxis();
4026         final double sx = ma.getElementAt(0, 0);
4027         final double sy = ma.getElementAt(1, 1);
4028         final double sz = ma.getElementAt(2, 2);
4029 
4030         final Acceleration bx = new Acceleration(biasX,
4031                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4032         final Acceleration by = new Acceleration(biasY,
4033                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4034         final Acceleration bz = new Acceleration(biasZ,
4035                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4036 
4037         final KnownGravityNormAccelerometerCalibrator calibrator =
4038                 new KnownGravityNormAccelerometerCalibrator(measurements,
4039                         true, bx, by, bz, sx, sy, sz,
4040                         this);
4041 
4042         // check default values
4043         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4044         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4045         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4046         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4047         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4048         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4049         final Acceleration bx2 = new Acceleration(0.0,
4050                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4051         calibrator.getInitialBiasXAsAcceleration(bx2);
4052         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4053         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4054         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4055         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4056         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4057         final Acceleration by2 = new Acceleration(0.0,
4058                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4059         calibrator.getInitialBiasYAsAcceleration(by2);
4060         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4061         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4062         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4063         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4064         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4065         final Acceleration bz2 = new Acceleration(0.0,
4066                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4067         calibrator.getInitialBiasZAsAcceleration(bz2);
4068         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4069         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4070         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4071         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4072         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4073         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
4074         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
4075         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
4076         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
4077         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
4078         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
4079         final double[] bias1 = calibrator.getInitialBias();
4080         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4081         final double[] bias2 = new double[3];
4082         calibrator.getInitialBias(bias2);
4083         assertArrayEquals(bias1, bias2, 0.0);
4084         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4085         assertEquals(b1, ba);
4086         final Matrix b2 = new Matrix(3, 1);
4087         calibrator.getInitialBiasAsMatrix(b2);
4088         assertEquals(b1, b2);
4089         final Matrix ma1 = calibrator.getInitialMa();
4090         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
4091         final Matrix ma2 = new Matrix(3, 3);
4092         calibrator.getInitialMa(ma2);
4093         assertEquals(ma1, ma2);
4094         assertSame(calibrator.getMeasurements(), measurements);
4095         assertTrue(calibrator.isCommonAxisUsed());
4096         assertSame(calibrator.getListener(), this);
4097         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4098         assertFalse(calibrator.isReady());
4099         assertFalse(calibrator.isRunning());
4100         assertNull(calibrator.getEstimatedBiases());
4101         assertFalse(calibrator.getEstimatedBiases(null));
4102         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4103         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4104         assertNull(calibrator.getEstimatedBiasFx());
4105         assertNull(calibrator.getEstimatedBiasFy());
4106         assertNull(calibrator.getEstimatedBiasFz());
4107         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4108         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4109         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4110         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4111         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4112         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4113         assertNull(calibrator.getEstimatedMa());
4114         assertNull(calibrator.getEstimatedSx());
4115         assertNull(calibrator.getEstimatedSy());
4116         assertNull(calibrator.getEstimatedSz());
4117         assertNull(calibrator.getEstimatedMxy());
4118         assertNull(calibrator.getEstimatedMxz());
4119         assertNull(calibrator.getEstimatedMyx());
4120         assertNull(calibrator.getEstimatedMyz());
4121         assertNull(calibrator.getEstimatedMzx());
4122         assertNull(calibrator.getEstimatedMzy());
4123         assertNull(calibrator.getEstimatedCovariance());
4124         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4125         assertNull(calibrator.getGroundTruthGravityNorm());
4126         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4127         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4128     }
4129 
4130     @Test
4131     public void testConstructor40() throws WrongSizeException {
4132         final Matrix ba = generateBa();
4133         final double biasX = ba.getElementAtIndex(0);
4134         final double biasY = ba.getElementAtIndex(1);
4135         final double biasZ = ba.getElementAtIndex(2);
4136 
4137         final Matrix ma = generateMaCommonAxis();
4138         final double sx = ma.getElementAt(0, 0);
4139         final double sy = ma.getElementAt(1, 1);
4140         final double sz = ma.getElementAt(2, 2);
4141         final double mxy = ma.getElementAt(0, 1);
4142         final double mxz = ma.getElementAt(0, 2);
4143         final double myx = ma.getElementAt(1, 0);
4144         final double myz = ma.getElementAt(1, 2);
4145         final double mzx = ma.getElementAt(2, 0);
4146         final double mzy = ma.getElementAt(2, 1);
4147 
4148         final KnownGravityNormAccelerometerCalibrator calibrator =
4149                 new KnownGravityNormAccelerometerCalibrator(
4150                         biasX, biasY, biasZ, sx, sy, sz,
4151                         mxy, mxz, myx, myz, mzx, mzy);
4152 
4153         // check default values
4154         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4155         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4156         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4157         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4158         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4159         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4160         final Acceleration bx2 = new Acceleration(0.0,
4161                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4162         calibrator.getInitialBiasXAsAcceleration(bx2);
4163         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4164         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4165         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4166         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4167         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4168         final Acceleration by2 = new Acceleration(0.0,
4169                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4170         calibrator.getInitialBiasYAsAcceleration(by2);
4171         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4172         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4173         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4174         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4175         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4176         final Acceleration bz2 = new Acceleration(0.0,
4177                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4178         calibrator.getInitialBiasZAsAcceleration(bz2);
4179         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4180         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4181         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4182         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4183         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4184         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4185         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4186         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4187         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4188         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4189         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4190         final double[] bias1 = calibrator.getInitialBias();
4191         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4192         final double[] bias2 = new double[3];
4193         calibrator.getInitialBias(bias2);
4194         assertArrayEquals(bias1, bias2, 0.0);
4195         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4196         assertEquals(b1, ba);
4197         final Matrix b2 = new Matrix(3, 1);
4198         calibrator.getInitialBiasAsMatrix(b2);
4199         assertEquals(b1, b2);
4200         final Matrix ma1 = new Matrix(3, 3);
4201         ma1.setSubmatrix(0, 0,
4202                 2, 2,
4203                 new double[]{sx, myx, mzx,
4204                         mxy, sy, mzy,
4205                         mxz, myz, sz});
4206         assertEquals(calibrator.getInitialMa(), ma1);
4207         final Matrix ma2 = new Matrix(3, 3);
4208         calibrator.getInitialMa(ma2);
4209         assertEquals(ma1, ma2);
4210         assertNull(calibrator.getMeasurements());
4211         assertFalse(calibrator.isCommonAxisUsed());
4212         assertNull(calibrator.getListener());
4213         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4214         assertFalse(calibrator.isReady());
4215         assertFalse(calibrator.isRunning());
4216         assertNull(calibrator.getEstimatedBiases());
4217         assertFalse(calibrator.getEstimatedBiases(null));
4218         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4219         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4220         assertNull(calibrator.getEstimatedBiasFx());
4221         assertNull(calibrator.getEstimatedBiasFy());
4222         assertNull(calibrator.getEstimatedBiasFz());
4223         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4224         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4225         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4226         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4227         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4228         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4229         assertNull(calibrator.getEstimatedMa());
4230         assertNull(calibrator.getEstimatedSx());
4231         assertNull(calibrator.getEstimatedSy());
4232         assertNull(calibrator.getEstimatedSz());
4233         assertNull(calibrator.getEstimatedMxy());
4234         assertNull(calibrator.getEstimatedMxz());
4235         assertNull(calibrator.getEstimatedMyx());
4236         assertNull(calibrator.getEstimatedMyz());
4237         assertNull(calibrator.getEstimatedMzx());
4238         assertNull(calibrator.getEstimatedMzy());
4239         assertNull(calibrator.getEstimatedCovariance());
4240         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4241         assertNull(calibrator.getGroundTruthGravityNorm());
4242         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4243         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4244     }
4245 
4246     @Test
4247     public void testConstructor41() throws WrongSizeException {
4248         final Collection<StandardDeviationBodyKinematics> measurements =
4249                 Collections.emptyList();
4250 
4251         final Matrix ba = generateBa();
4252         final double biasX = ba.getElementAtIndex(0);
4253         final double biasY = ba.getElementAtIndex(1);
4254         final double biasZ = ba.getElementAtIndex(2);
4255 
4256         final Matrix ma = generateMaCommonAxis();
4257         final double sx = ma.getElementAt(0, 0);
4258         final double sy = ma.getElementAt(1, 1);
4259         final double sz = ma.getElementAt(2, 2);
4260         final double mxy = ma.getElementAt(0, 1);
4261         final double mxz = ma.getElementAt(0, 2);
4262         final double myx = ma.getElementAt(1, 0);
4263         final double myz = ma.getElementAt(1, 2);
4264         final double mzx = ma.getElementAt(2, 0);
4265         final double mzy = ma.getElementAt(2, 1);
4266 
4267         final KnownGravityNormAccelerometerCalibrator calibrator =
4268                 new KnownGravityNormAccelerometerCalibrator(measurements,
4269                         biasX, biasY, biasZ, sx, sy, sz,
4270                         mxy, mxz, myx, myz, mzx, mzy);
4271 
4272         // check default values
4273         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4274         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4275         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4276         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4277         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4278         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4279         final Acceleration bx2 = new Acceleration(0.0,
4280                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4281         calibrator.getInitialBiasXAsAcceleration(bx2);
4282         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4283         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4284         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4285         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4286         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4287         final Acceleration by2 = new Acceleration(0.0,
4288                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4289         calibrator.getInitialBiasYAsAcceleration(by2);
4290         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4291         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4292         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4293         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4294         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4295         final Acceleration bz2 = new Acceleration(0.0,
4296                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4297         calibrator.getInitialBiasZAsAcceleration(bz2);
4298         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4299         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4300         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4301         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4302         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4303         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4304         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4305         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4306         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4307         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4308         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4309         final double[] bias1 = calibrator.getInitialBias();
4310         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4311         final double[] bias2 = new double[3];
4312         calibrator.getInitialBias(bias2);
4313         assertArrayEquals(bias1, bias2, 0.0);
4314         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4315         assertEquals(b1, ba);
4316         final Matrix b2 = new Matrix(3, 1);
4317         calibrator.getInitialBiasAsMatrix(b2);
4318         assertEquals(b1, b2);
4319         final Matrix ma1 = new Matrix(3, 3);
4320         ma1.setSubmatrix(0, 0,
4321                 2, 2,
4322                 new double[]{sx, myx, mzx,
4323                         mxy, sy, mzy,
4324                         mxz, myz, sz});
4325         assertEquals(calibrator.getInitialMa(), ma1);
4326         final Matrix ma2 = new Matrix(3, 3);
4327         calibrator.getInitialMa(ma2);
4328         assertEquals(ma1, ma2);
4329         assertSame(calibrator.getMeasurements(), measurements);
4330         assertFalse(calibrator.isCommonAxisUsed());
4331         assertNull(calibrator.getListener());
4332         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4333         assertFalse(calibrator.isReady());
4334         assertFalse(calibrator.isRunning());
4335         assertNull(calibrator.getEstimatedBiases());
4336         assertFalse(calibrator.getEstimatedBiases(null));
4337         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4338         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4339         assertNull(calibrator.getEstimatedBiasFx());
4340         assertNull(calibrator.getEstimatedBiasFy());
4341         assertNull(calibrator.getEstimatedBiasFz());
4342         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4343         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4344         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4345         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4346         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4347         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4348         assertNull(calibrator.getEstimatedMa());
4349         assertNull(calibrator.getEstimatedSx());
4350         assertNull(calibrator.getEstimatedSy());
4351         assertNull(calibrator.getEstimatedSz());
4352         assertNull(calibrator.getEstimatedMxy());
4353         assertNull(calibrator.getEstimatedMxz());
4354         assertNull(calibrator.getEstimatedMyx());
4355         assertNull(calibrator.getEstimatedMyz());
4356         assertNull(calibrator.getEstimatedMzx());
4357         assertNull(calibrator.getEstimatedMzy());
4358         assertNull(calibrator.getEstimatedCovariance());
4359         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4360         assertNull(calibrator.getGroundTruthGravityNorm());
4361         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4362         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4363     }
4364 
4365     @Test
4366     public void testConstructor42() throws WrongSizeException {
4367         final Collection<StandardDeviationBodyKinematics> measurements =
4368                 Collections.emptyList();
4369 
4370         final Matrix ba = generateBa();
4371         final double biasX = ba.getElementAtIndex(0);
4372         final double biasY = ba.getElementAtIndex(1);
4373         final double biasZ = ba.getElementAtIndex(2);
4374 
4375         final Matrix ma = generateMaCommonAxis();
4376         final double sx = ma.getElementAt(0, 0);
4377         final double sy = ma.getElementAt(1, 1);
4378         final double sz = ma.getElementAt(2, 2);
4379         final double mxy = ma.getElementAt(0, 1);
4380         final double mxz = ma.getElementAt(0, 2);
4381         final double myx = ma.getElementAt(1, 0);
4382         final double myz = ma.getElementAt(1, 2);
4383         final double mzx = ma.getElementAt(2, 0);
4384         final double mzy = ma.getElementAt(2, 1);
4385 
4386         final KnownGravityNormAccelerometerCalibrator calibrator =
4387                 new KnownGravityNormAccelerometerCalibrator(measurements,
4388                         biasX, biasY, biasZ, sx, sy, sz,
4389                         mxy, mxz, myx, myz, mzx, mzy, this);
4390 
4391         // check default values
4392         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4393         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4394         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4395         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4396         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4397         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4398         final Acceleration bx2 = new Acceleration(0.0,
4399                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4400         calibrator.getInitialBiasXAsAcceleration(bx2);
4401         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4402         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4403         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4404         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4405         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4406         final Acceleration by2 = new Acceleration(0.0,
4407                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4408         calibrator.getInitialBiasYAsAcceleration(by2);
4409         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4410         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4411         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4412         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4413         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4414         final Acceleration bz2 = new Acceleration(0.0,
4415                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4416         calibrator.getInitialBiasZAsAcceleration(bz2);
4417         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4418         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4419         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4420         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4421         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4422         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4423         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4424         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4425         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4426         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4427         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4428         final double[] bias1 = calibrator.getInitialBias();
4429         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4430         final double[] bias2 = new double[3];
4431         calibrator.getInitialBias(bias2);
4432         assertArrayEquals(bias1, bias2, 0.0);
4433         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4434         assertEquals(b1, ba);
4435         final Matrix b2 = new Matrix(3, 1);
4436         calibrator.getInitialBiasAsMatrix(b2);
4437         assertEquals(b1, b2);
4438         final Matrix ma1 = new Matrix(3, 3);
4439         ma1.setSubmatrix(0, 0,
4440                 2, 2,
4441                 new double[]{sx, myx, mzx,
4442                         mxy, sy, mzy,
4443                         mxz, myz, sz});
4444         assertEquals(calibrator.getInitialMa(), ma1);
4445         final Matrix ma2 = new Matrix(3, 3);
4446         calibrator.getInitialMa(ma2);
4447         assertEquals(ma1, ma2);
4448         assertSame(calibrator.getMeasurements(), measurements);
4449         assertFalse(calibrator.isCommonAxisUsed());
4450         assertSame(calibrator.getListener(), this);
4451         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4452         assertFalse(calibrator.isReady());
4453         assertFalse(calibrator.isRunning());
4454         assertNull(calibrator.getEstimatedBiases());
4455         assertFalse(calibrator.getEstimatedBiases(null));
4456         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4457         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4458         assertNull(calibrator.getEstimatedBiasFx());
4459         assertNull(calibrator.getEstimatedBiasFy());
4460         assertNull(calibrator.getEstimatedBiasFz());
4461         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4462         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4463         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4464         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4465         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4466         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4467         assertNull(calibrator.getEstimatedMa());
4468         assertNull(calibrator.getEstimatedSx());
4469         assertNull(calibrator.getEstimatedSy());
4470         assertNull(calibrator.getEstimatedSz());
4471         assertNull(calibrator.getEstimatedMxy());
4472         assertNull(calibrator.getEstimatedMxz());
4473         assertNull(calibrator.getEstimatedMyx());
4474         assertNull(calibrator.getEstimatedMyz());
4475         assertNull(calibrator.getEstimatedMzx());
4476         assertNull(calibrator.getEstimatedMzy());
4477         assertNull(calibrator.getEstimatedCovariance());
4478         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4479         assertNull(calibrator.getGroundTruthGravityNorm());
4480         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4481         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4482     }
4483 
4484     @Test
4485     public void testConstructor43() throws WrongSizeException {
4486         final Matrix ba = generateBa();
4487         final double biasX = ba.getElementAtIndex(0);
4488         final double biasY = ba.getElementAtIndex(1);
4489         final double biasZ = ba.getElementAtIndex(2);
4490 
4491         final Matrix ma = generateMaCommonAxis();
4492         final double sx = ma.getElementAt(0, 0);
4493         final double sy = ma.getElementAt(1, 1);
4494         final double sz = ma.getElementAt(2, 2);
4495         final double mxy = ma.getElementAt(0, 1);
4496         final double mxz = ma.getElementAt(0, 2);
4497         final double myx = ma.getElementAt(1, 0);
4498         final double myz = ma.getElementAt(1, 2);
4499         final double mzx = ma.getElementAt(2, 0);
4500         final double mzy = ma.getElementAt(2, 1);
4501 
4502         final KnownGravityNormAccelerometerCalibrator calibrator =
4503                 new KnownGravityNormAccelerometerCalibrator(
4504                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
4505                         myz, mzx, mzy);
4506 
4507         // check default values
4508         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4509         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4510         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4511         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4512         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4513         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4514         final Acceleration bx2 = new Acceleration(0.0,
4515                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4516         calibrator.getInitialBiasXAsAcceleration(bx2);
4517         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4518         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4519         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4520         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4521         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4522         final Acceleration by2 = new Acceleration(0.0,
4523                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4524         calibrator.getInitialBiasYAsAcceleration(by2);
4525         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4526         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4527         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4528         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4529         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4530         final Acceleration bz2 = new Acceleration(0.0,
4531                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4532         calibrator.getInitialBiasZAsAcceleration(bz2);
4533         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4534         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4535         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4536         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4537         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4538         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4539         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4540         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4541         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4542         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4543         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4544         final double[] bias1 = calibrator.getInitialBias();
4545         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4546         final double[] bias2 = new double[3];
4547         calibrator.getInitialBias(bias2);
4548         assertArrayEquals(bias1, bias2, 0.0);
4549         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4550         assertEquals(b1, ba);
4551         final Matrix b2 = new Matrix(3, 1);
4552         calibrator.getInitialBiasAsMatrix(b2);
4553         assertEquals(b1, b2);
4554         final Matrix ma1 = new Matrix(3, 3);
4555         ma1.setSubmatrix(0, 0,
4556                 2, 2,
4557                 new double[]{sx, myx, mzx,
4558                         mxy, sy, mzy,
4559                         mxz, myz, sz});
4560         assertEquals(calibrator.getInitialMa(), ma1);
4561         final Matrix ma2 = new Matrix(3, 3);
4562         calibrator.getInitialMa(ma2);
4563         assertEquals(ma1, ma2);
4564         assertNull(calibrator.getMeasurements());
4565         assertTrue(calibrator.isCommonAxisUsed());
4566         assertNull(calibrator.getListener());
4567         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4568         assertFalse(calibrator.isReady());
4569         assertFalse(calibrator.isRunning());
4570         assertNull(calibrator.getEstimatedBiases());
4571         assertFalse(calibrator.getEstimatedBiases(null));
4572         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4573         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4574         assertNull(calibrator.getEstimatedBiasFx());
4575         assertNull(calibrator.getEstimatedBiasFy());
4576         assertNull(calibrator.getEstimatedBiasFz());
4577         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4578         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4579         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4580         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4581         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4582         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4583         assertNull(calibrator.getEstimatedMa());
4584         assertNull(calibrator.getEstimatedSx());
4585         assertNull(calibrator.getEstimatedSy());
4586         assertNull(calibrator.getEstimatedSz());
4587         assertNull(calibrator.getEstimatedMxy());
4588         assertNull(calibrator.getEstimatedMxz());
4589         assertNull(calibrator.getEstimatedMyx());
4590         assertNull(calibrator.getEstimatedMyz());
4591         assertNull(calibrator.getEstimatedMzx());
4592         assertNull(calibrator.getEstimatedMzy());
4593         assertNull(calibrator.getEstimatedCovariance());
4594         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4595         assertNull(calibrator.getGroundTruthGravityNorm());
4596         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4597         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4598     }
4599 
4600     @Test
4601     public void testConstructor44() throws WrongSizeException {
4602         final Matrix ba = generateBa();
4603         final double biasX = ba.getElementAtIndex(0);
4604         final double biasY = ba.getElementAtIndex(1);
4605         final double biasZ = ba.getElementAtIndex(2);
4606 
4607         final Matrix ma = generateMaCommonAxis();
4608         final double sx = ma.getElementAt(0, 0);
4609         final double sy = ma.getElementAt(1, 1);
4610         final double sz = ma.getElementAt(2, 2);
4611         final double mxy = ma.getElementAt(0, 1);
4612         final double mxz = ma.getElementAt(0, 2);
4613         final double myx = ma.getElementAt(1, 0);
4614         final double myz = ma.getElementAt(1, 2);
4615         final double mzx = ma.getElementAt(2, 0);
4616         final double mzy = ma.getElementAt(2, 1);
4617 
4618         final KnownGravityNormAccelerometerCalibrator calibrator =
4619                 new KnownGravityNormAccelerometerCalibrator(
4620                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
4621                         myz, mzx, mzy, this);
4622 
4623         // check default values
4624         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4625         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4626         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4627         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4628         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4629         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4630         final Acceleration bx2 = new Acceleration(0.0,
4631                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4632         calibrator.getInitialBiasXAsAcceleration(bx2);
4633         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4634         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4635         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4636         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4637         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4638         final Acceleration by2 = new Acceleration(0.0,
4639                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4640         calibrator.getInitialBiasYAsAcceleration(by2);
4641         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4642         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4643         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4644         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4645         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4646         final Acceleration bz2 = new Acceleration(0.0,
4647                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4648         calibrator.getInitialBiasZAsAcceleration(bz2);
4649         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4650         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4651         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4652         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4653         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4654         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4655         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4656         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4657         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4658         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4659         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4660         final double[] bias1 = calibrator.getInitialBias();
4661         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4662         final double[] bias2 = new double[3];
4663         calibrator.getInitialBias(bias2);
4664         assertArrayEquals(bias1, bias2, 0.0);
4665         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4666         assertEquals(b1, ba);
4667         final Matrix b2 = new Matrix(3, 1);
4668         calibrator.getInitialBiasAsMatrix(b2);
4669         assertEquals(b1, b2);
4670         final Matrix ma1 = new Matrix(3, 3);
4671         ma1.setSubmatrix(0, 0,
4672                 2, 2,
4673                 new double[]{sx, myx, mzx,
4674                         mxy, sy, mzy,
4675                         mxz, myz, sz});
4676         assertEquals(calibrator.getInitialMa(), ma1);
4677         final Matrix ma2 = new Matrix(3, 3);
4678         calibrator.getInitialMa(ma2);
4679         assertEquals(ma1, ma2);
4680         assertNull(calibrator.getMeasurements());
4681         assertTrue(calibrator.isCommonAxisUsed());
4682         assertSame(calibrator.getListener(), this);
4683         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4684         assertFalse(calibrator.isReady());
4685         assertFalse(calibrator.isRunning());
4686         assertNull(calibrator.getEstimatedBiases());
4687         assertFalse(calibrator.getEstimatedBiases(null));
4688         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4689         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4690         assertNull(calibrator.getEstimatedBiasFx());
4691         assertNull(calibrator.getEstimatedBiasFy());
4692         assertNull(calibrator.getEstimatedBiasFz());
4693         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4694         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4695         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4696         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4697         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4698         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4699         assertNull(calibrator.getEstimatedMa());
4700         assertNull(calibrator.getEstimatedSx());
4701         assertNull(calibrator.getEstimatedSy());
4702         assertNull(calibrator.getEstimatedSz());
4703         assertNull(calibrator.getEstimatedMxy());
4704         assertNull(calibrator.getEstimatedMxz());
4705         assertNull(calibrator.getEstimatedMyx());
4706         assertNull(calibrator.getEstimatedMyz());
4707         assertNull(calibrator.getEstimatedMzx());
4708         assertNull(calibrator.getEstimatedMzy());
4709         assertNull(calibrator.getEstimatedCovariance());
4710         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4711         assertNull(calibrator.getGroundTruthGravityNorm());
4712         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4713         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4714     }
4715 
4716     @Test
4717     public void testConstructor45() throws WrongSizeException {
4718         final Collection<StandardDeviationBodyKinematics> measurements =
4719                 Collections.emptyList();
4720 
4721         final Matrix ba = generateBa();
4722         final double biasX = ba.getElementAtIndex(0);
4723         final double biasY = ba.getElementAtIndex(1);
4724         final double biasZ = ba.getElementAtIndex(2);
4725 
4726         final Matrix ma = generateMaCommonAxis();
4727         final double sx = ma.getElementAt(0, 0);
4728         final double sy = ma.getElementAt(1, 1);
4729         final double sz = ma.getElementAt(2, 2);
4730         final double mxy = ma.getElementAt(0, 1);
4731         final double mxz = ma.getElementAt(0, 2);
4732         final double myx = ma.getElementAt(1, 0);
4733         final double myz = ma.getElementAt(1, 2);
4734         final double mzx = ma.getElementAt(2, 0);
4735         final double mzy = ma.getElementAt(2, 1);
4736 
4737         final KnownGravityNormAccelerometerCalibrator calibrator =
4738                 new KnownGravityNormAccelerometerCalibrator(measurements,
4739                         true, biasX, biasY, biasZ,
4740                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4741 
4742         // check default values
4743         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4744         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4745         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4746         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4747         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4748         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4749         final Acceleration bx2 = new Acceleration(0.0,
4750                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4751         calibrator.getInitialBiasXAsAcceleration(bx2);
4752         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4753         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4754         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4755         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4756         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4757         final Acceleration by2 = new Acceleration(0.0,
4758                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4759         calibrator.getInitialBiasYAsAcceleration(by2);
4760         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4761         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4762         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4763         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4764         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4765         final Acceleration bz2 = new Acceleration(0.0,
4766                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4767         calibrator.getInitialBiasZAsAcceleration(bz2);
4768         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4769         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4770         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4771         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4772         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4773         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4774         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4775         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4776         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4777         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4778         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4779         final double[] bias1 = calibrator.getInitialBias();
4780         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4781         final double[] bias2 = new double[3];
4782         calibrator.getInitialBias(bias2);
4783         assertArrayEquals(bias1, bias2, 0.0);
4784         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4785         assertEquals(b1, ba);
4786         final Matrix b2 = new Matrix(3, 1);
4787         calibrator.getInitialBiasAsMatrix(b2);
4788         assertEquals(b1, b2);
4789         final Matrix ma1 = new Matrix(3, 3);
4790         ma1.setSubmatrix(0, 0,
4791                 2, 2,
4792                 new double[]{sx, myx, mzx,
4793                         mxy, sy, mzy,
4794                         mxz, myz, sz});
4795         assertEquals(calibrator.getInitialMa(), ma1);
4796         final Matrix ma2 = new Matrix(3, 3);
4797         calibrator.getInitialMa(ma2);
4798         assertEquals(ma1, ma2);
4799         assertSame(calibrator.getMeasurements(), measurements);
4800         assertTrue(calibrator.isCommonAxisUsed());
4801         assertNull(calibrator.getListener());
4802         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4803         assertFalse(calibrator.isReady());
4804         assertFalse(calibrator.isRunning());
4805         assertNull(calibrator.getEstimatedBiases());
4806         assertFalse(calibrator.getEstimatedBiases(null));
4807         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4808         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4809         assertNull(calibrator.getEstimatedBiasFx());
4810         assertNull(calibrator.getEstimatedBiasFy());
4811         assertNull(calibrator.getEstimatedBiasFz());
4812         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4813         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4814         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4815         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4816         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4817         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4818         assertNull(calibrator.getEstimatedMa());
4819         assertNull(calibrator.getEstimatedSx());
4820         assertNull(calibrator.getEstimatedSy());
4821         assertNull(calibrator.getEstimatedSz());
4822         assertNull(calibrator.getEstimatedMxy());
4823         assertNull(calibrator.getEstimatedMxz());
4824         assertNull(calibrator.getEstimatedMyx());
4825         assertNull(calibrator.getEstimatedMyz());
4826         assertNull(calibrator.getEstimatedMzx());
4827         assertNull(calibrator.getEstimatedMzy());
4828         assertNull(calibrator.getEstimatedCovariance());
4829         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4830         assertNull(calibrator.getGroundTruthGravityNorm());
4831         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4832         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4833     }
4834 
4835     @Test
4836     public void testConstructor46() throws WrongSizeException {
4837         final Collection<StandardDeviationBodyKinematics> measurements =
4838                 Collections.emptyList();
4839 
4840         final Matrix ba = generateBa();
4841         final double biasX = ba.getElementAtIndex(0);
4842         final double biasY = ba.getElementAtIndex(1);
4843         final double biasZ = ba.getElementAtIndex(2);
4844 
4845         final Matrix ma = generateMaCommonAxis();
4846         final double sx = ma.getElementAt(0, 0);
4847         final double sy = ma.getElementAt(1, 1);
4848         final double sz = ma.getElementAt(2, 2);
4849         final double mxy = ma.getElementAt(0, 1);
4850         final double mxz = ma.getElementAt(0, 2);
4851         final double myx = ma.getElementAt(1, 0);
4852         final double myz = ma.getElementAt(1, 2);
4853         final double mzx = ma.getElementAt(2, 0);
4854         final double mzy = ma.getElementAt(2, 1);
4855 
4856         final KnownGravityNormAccelerometerCalibrator calibrator =
4857                 new KnownGravityNormAccelerometerCalibrator(measurements,
4858                         true, biasX, biasY, biasZ,
4859                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
4860 
4861         // check default values
4862         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4863         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4864         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4865         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4866         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4867         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4868         final Acceleration bx2 = new Acceleration(0.0,
4869                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4870         calibrator.getInitialBiasXAsAcceleration(bx2);
4871         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4872         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4873         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4874         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4875         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4876         final Acceleration by2 = new Acceleration(0.0,
4877                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4878         calibrator.getInitialBiasYAsAcceleration(by2);
4879         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4880         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4881         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4882         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4883         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4884         final Acceleration bz2 = new Acceleration(0.0,
4885                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4886         calibrator.getInitialBiasZAsAcceleration(bz2);
4887         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4888         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4889         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4890         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4891         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4892         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4893         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4894         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4895         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4896         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4897         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4898         final double[] bias1 = calibrator.getInitialBias();
4899         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4900         final double[] bias2 = new double[3];
4901         calibrator.getInitialBias(bias2);
4902         assertArrayEquals(bias1, bias2, 0.0);
4903         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4904         assertEquals(b1, ba);
4905         final Matrix b2 = new Matrix(3, 1);
4906         calibrator.getInitialBiasAsMatrix(b2);
4907         assertEquals(b1, b2);
4908         final Matrix ma1 = new Matrix(3, 3);
4909         ma1.setSubmatrix(0, 0,
4910                 2, 2,
4911                 new double[]{sx, myx, mzx,
4912                         mxy, sy, mzy,
4913                         mxz, myz, sz});
4914         assertEquals(calibrator.getInitialMa(), ma1);
4915         final Matrix ma2 = new Matrix(3, 3);
4916         calibrator.getInitialMa(ma2);
4917         assertEquals(ma1, ma2);
4918         assertSame(calibrator.getMeasurements(), measurements);
4919         assertTrue(calibrator.isCommonAxisUsed());
4920         assertSame(calibrator.getListener(), this);
4921         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4922         assertFalse(calibrator.isReady());
4923         assertFalse(calibrator.isRunning());
4924         assertNull(calibrator.getEstimatedBiases());
4925         assertFalse(calibrator.getEstimatedBiases(null));
4926         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4927         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4928         assertNull(calibrator.getEstimatedBiasFx());
4929         assertNull(calibrator.getEstimatedBiasFy());
4930         assertNull(calibrator.getEstimatedBiasFz());
4931         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4932         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4933         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4934         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4935         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4936         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4937         assertNull(calibrator.getEstimatedMa());
4938         assertNull(calibrator.getEstimatedSx());
4939         assertNull(calibrator.getEstimatedSy());
4940         assertNull(calibrator.getEstimatedSz());
4941         assertNull(calibrator.getEstimatedMxy());
4942         assertNull(calibrator.getEstimatedMxz());
4943         assertNull(calibrator.getEstimatedMyx());
4944         assertNull(calibrator.getEstimatedMyz());
4945         assertNull(calibrator.getEstimatedMzx());
4946         assertNull(calibrator.getEstimatedMzy());
4947         assertNull(calibrator.getEstimatedCovariance());
4948         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4949         assertNull(calibrator.getGroundTruthGravityNorm());
4950         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4951         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4952     }
4953 
4954     @Test
4955     public void testConstructor47() throws WrongSizeException {
4956         final Matrix ba = generateBa();
4957         final double biasX = ba.getElementAtIndex(0);
4958         final double biasY = ba.getElementAtIndex(1);
4959         final double biasZ = ba.getElementAtIndex(2);
4960 
4961         final Matrix ma = generateMaCommonAxis();
4962         final double sx = ma.getElementAt(0, 0);
4963         final double sy = ma.getElementAt(1, 1);
4964         final double sz = ma.getElementAt(2, 2);
4965         final double mxy = ma.getElementAt(0, 1);
4966         final double mxz = ma.getElementAt(0, 2);
4967         final double myx = ma.getElementAt(1, 0);
4968         final double myz = ma.getElementAt(1, 2);
4969         final double mzx = ma.getElementAt(2, 0);
4970         final double mzy = ma.getElementAt(2, 1);
4971 
4972         final Acceleration bx = new Acceleration(biasX,
4973                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4974         final Acceleration by = new Acceleration(biasY,
4975                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4976         final Acceleration bz = new Acceleration(biasZ,
4977                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4978 
4979         final KnownGravityNormAccelerometerCalibrator calibrator =
4980                 new KnownGravityNormAccelerometerCalibrator(bx, by, bz,
4981                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4982 
4983         // check default values
4984         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4985         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4986         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4987         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4988         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4989         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4990         final Acceleration bx2 = new Acceleration(0.0,
4991                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4992         calibrator.getInitialBiasXAsAcceleration(bx2);
4993         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4994         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4995         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4996         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4997         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4998         final Acceleration by2 = new Acceleration(0.0,
4999                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5000         calibrator.getInitialBiasYAsAcceleration(by2);
5001         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5002         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5003         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5004         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5005         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5006         final Acceleration bz2 = new Acceleration(0.0,
5007                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5008         calibrator.getInitialBiasZAsAcceleration(bz2);
5009         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5010         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5011         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5012         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5013         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5014         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5015         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5016         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5017         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5018         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5019         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5020         final double[] bias1 = calibrator.getInitialBias();
5021         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5022         final double[] bias2 = new double[3];
5023         calibrator.getInitialBias(bias2);
5024         assertArrayEquals(bias1, bias2, 0.0);
5025         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5026         assertEquals(b1, ba);
5027         final Matrix b2 = new Matrix(3, 1);
5028         calibrator.getInitialBiasAsMatrix(b2);
5029         assertEquals(b1, b2);
5030         final Matrix ma1 = new Matrix(3, 3);
5031         ma1.setSubmatrix(0, 0,
5032                 2, 2,
5033                 new double[]{sx, myx, mzx,
5034                         mxy, sy, mzy,
5035                         mxz, myz, sz});
5036         assertEquals(calibrator.getInitialMa(), ma1);
5037         final Matrix ma2 = new Matrix(3, 3);
5038         calibrator.getInitialMa(ma2);
5039         assertEquals(ma1, ma2);
5040         assertNull(calibrator.getMeasurements());
5041         assertFalse(calibrator.isCommonAxisUsed());
5042         assertNull(calibrator.getListener());
5043         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5044         assertFalse(calibrator.isReady());
5045         assertFalse(calibrator.isRunning());
5046         assertNull(calibrator.getEstimatedBiases());
5047         assertFalse(calibrator.getEstimatedBiases(null));
5048         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5049         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5050         assertNull(calibrator.getEstimatedBiasFx());
5051         assertNull(calibrator.getEstimatedBiasFy());
5052         assertNull(calibrator.getEstimatedBiasFz());
5053         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5054         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5055         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5056         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5057         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5058         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5059         assertNull(calibrator.getEstimatedMa());
5060         assertNull(calibrator.getEstimatedSx());
5061         assertNull(calibrator.getEstimatedSy());
5062         assertNull(calibrator.getEstimatedSz());
5063         assertNull(calibrator.getEstimatedMxy());
5064         assertNull(calibrator.getEstimatedMxz());
5065         assertNull(calibrator.getEstimatedMyx());
5066         assertNull(calibrator.getEstimatedMyz());
5067         assertNull(calibrator.getEstimatedMzx());
5068         assertNull(calibrator.getEstimatedMzy());
5069         assertNull(calibrator.getEstimatedCovariance());
5070         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5071         assertNull(calibrator.getGroundTruthGravityNorm());
5072         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5073         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5074     }
5075 
5076     @Test
5077     public void testConstructor48() throws WrongSizeException {
5078         final Matrix ba = generateBa();
5079         final double biasX = ba.getElementAtIndex(0);
5080         final double biasY = ba.getElementAtIndex(1);
5081         final double biasZ = ba.getElementAtIndex(2);
5082 
5083         final Matrix ma = generateMaCommonAxis();
5084         final double sx = ma.getElementAt(0, 0);
5085         final double sy = ma.getElementAt(1, 1);
5086         final double sz = ma.getElementAt(2, 2);
5087         final double mxy = ma.getElementAt(0, 1);
5088         final double mxz = ma.getElementAt(0, 2);
5089         final double myx = ma.getElementAt(1, 0);
5090         final double myz = ma.getElementAt(1, 2);
5091         final double mzx = ma.getElementAt(2, 0);
5092         final double mzy = ma.getElementAt(2, 1);
5093 
5094         final Acceleration bx = new Acceleration(biasX,
5095                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5096         final Acceleration by = new Acceleration(biasY,
5097                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5098         final Acceleration bz = new Acceleration(biasZ,
5099                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5100 
5101         final KnownGravityNormAccelerometerCalibrator calibrator =
5102                 new KnownGravityNormAccelerometerCalibrator(bx, by, bz,
5103                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
5104 
5105         // check default values
5106         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5107         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5108         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5109         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5110         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5111         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5112         final Acceleration bx2 = new Acceleration(0.0,
5113                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5114         calibrator.getInitialBiasXAsAcceleration(bx2);
5115         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5116         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5117         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5118         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5119         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5120         final Acceleration by2 = new Acceleration(0.0,
5121                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5122         calibrator.getInitialBiasYAsAcceleration(by2);
5123         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5124         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5125         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5126         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5127         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5128         final Acceleration bz2 = new Acceleration(0.0,
5129                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5130         calibrator.getInitialBiasZAsAcceleration(bz2);
5131         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5132         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5133         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5134         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5135         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5136         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5137         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5138         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5139         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5140         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5141         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5142         final double[] bias1 = calibrator.getInitialBias();
5143         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5144         final double[] bias2 = new double[3];
5145         calibrator.getInitialBias(bias2);
5146         assertArrayEquals(bias1, bias2, 0.0);
5147         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5148         assertEquals(b1, ba);
5149         final Matrix b2 = new Matrix(3, 1);
5150         calibrator.getInitialBiasAsMatrix(b2);
5151         assertEquals(b1, b2);
5152         final Matrix ma1 = new Matrix(3, 3);
5153         ma1.setSubmatrix(0, 0,
5154                 2, 2,
5155                 new double[]{sx, myx, mzx,
5156                         mxy, sy, mzy,
5157                         mxz, myz, sz});
5158         assertEquals(calibrator.getInitialMa(), ma1);
5159         final Matrix ma2 = new Matrix(3, 3);
5160         calibrator.getInitialMa(ma2);
5161         assertEquals(ma1, ma2);
5162         assertNull(calibrator.getMeasurements());
5163         assertFalse(calibrator.isCommonAxisUsed());
5164         assertSame(calibrator.getListener(), this);
5165         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5166         assertFalse(calibrator.isReady());
5167         assertFalse(calibrator.isRunning());
5168         assertNull(calibrator.getEstimatedBiases());
5169         assertFalse(calibrator.getEstimatedBiases(null));
5170         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5171         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5172         assertNull(calibrator.getEstimatedBiasFx());
5173         assertNull(calibrator.getEstimatedBiasFy());
5174         assertNull(calibrator.getEstimatedBiasFz());
5175         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5176         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5177         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5178         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5179         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5180         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5181         assertNull(calibrator.getEstimatedMa());
5182         assertNull(calibrator.getEstimatedSx());
5183         assertNull(calibrator.getEstimatedSy());
5184         assertNull(calibrator.getEstimatedSz());
5185         assertNull(calibrator.getEstimatedMxy());
5186         assertNull(calibrator.getEstimatedMxz());
5187         assertNull(calibrator.getEstimatedMyx());
5188         assertNull(calibrator.getEstimatedMyz());
5189         assertNull(calibrator.getEstimatedMzx());
5190         assertNull(calibrator.getEstimatedMzy());
5191         assertNull(calibrator.getEstimatedCovariance());
5192         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5193         assertNull(calibrator.getGroundTruthGravityNorm());
5194         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5195         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5196     }
5197 
5198     @Test
5199     public void testConstructor49() throws WrongSizeException {
5200 
5201         final Collection<StandardDeviationBodyKinematics> measurements =
5202                 Collections.emptyList();
5203 
5204         final Matrix ba = generateBa();
5205         final double biasX = ba.getElementAtIndex(0);
5206         final double biasY = ba.getElementAtIndex(1);
5207         final double biasZ = ba.getElementAtIndex(2);
5208 
5209         final Matrix ma = generateMaCommonAxis();
5210         final double sx = ma.getElementAt(0, 0);
5211         final double sy = ma.getElementAt(1, 1);
5212         final double sz = ma.getElementAt(2, 2);
5213         final double mxy = ma.getElementAt(0, 1);
5214         final double mxz = ma.getElementAt(0, 2);
5215         final double myx = ma.getElementAt(1, 0);
5216         final double myz = ma.getElementAt(1, 2);
5217         final double mzx = ma.getElementAt(2, 0);
5218         final double mzy = ma.getElementAt(2, 1);
5219 
5220         final Acceleration bx = new Acceleration(biasX,
5221                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5222         final Acceleration by = new Acceleration(biasY,
5223                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5224         final Acceleration bz = new Acceleration(biasZ,
5225                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5226 
5227         final KnownGravityNormAccelerometerCalibrator calibrator =
5228                 new KnownGravityNormAccelerometerCalibrator(
5229                         measurements, bx, by, bz, sx, sy, sz,
5230                         mxy, mxz, myx, myz, mzx, mzy);
5231 
5232         // check default values
5233         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5234         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5235         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5236         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5237         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5238         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5239         final Acceleration bx2 = new Acceleration(0.0,
5240                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5241         calibrator.getInitialBiasXAsAcceleration(bx2);
5242         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5243         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5244         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5245         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5246         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5247         final Acceleration by2 = new Acceleration(0.0,
5248                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5249         calibrator.getInitialBiasYAsAcceleration(by2);
5250         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5251         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5252         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5253         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5254         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5255         final Acceleration bz2 = new Acceleration(0.0,
5256                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5257         calibrator.getInitialBiasZAsAcceleration(bz2);
5258         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5259         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5260         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5261         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5262         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5263         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5264         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5265         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5266         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5267         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5268         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5269         final double[] bias1 = calibrator.getInitialBias();
5270         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5271         final double[] bias2 = new double[3];
5272         calibrator.getInitialBias(bias2);
5273         assertArrayEquals(bias1, bias2, 0.0);
5274         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5275         assertEquals(b1, ba);
5276         final Matrix b2 = new Matrix(3, 1);
5277         calibrator.getInitialBiasAsMatrix(b2);
5278         assertEquals(b1, b2);
5279         final Matrix ma1 = new Matrix(3, 3);
5280         ma1.setSubmatrix(0, 0,
5281                 2, 2,
5282                 new double[]{sx, myx, mzx,
5283                         mxy, sy, mzy,
5284                         mxz, myz, sz});
5285         assertEquals(calibrator.getInitialMa(), ma1);
5286         final Matrix ma2 = new Matrix(3, 3);
5287         calibrator.getInitialMa(ma2);
5288         assertEquals(ma1, ma2);
5289         assertSame(calibrator.getMeasurements(), measurements);
5290         assertFalse(calibrator.isCommonAxisUsed());
5291         assertNull(calibrator.getListener());
5292         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5293         assertFalse(calibrator.isReady());
5294         assertFalse(calibrator.isRunning());
5295         assertNull(calibrator.getEstimatedBiases());
5296         assertFalse(calibrator.getEstimatedBiases(null));
5297         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5298         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5299         assertNull(calibrator.getEstimatedBiasFx());
5300         assertNull(calibrator.getEstimatedBiasFy());
5301         assertNull(calibrator.getEstimatedBiasFz());
5302         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5303         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5304         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5305         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5306         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5307         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5308         assertNull(calibrator.getEstimatedMa());
5309         assertNull(calibrator.getEstimatedSx());
5310         assertNull(calibrator.getEstimatedSy());
5311         assertNull(calibrator.getEstimatedSz());
5312         assertNull(calibrator.getEstimatedMxy());
5313         assertNull(calibrator.getEstimatedMxz());
5314         assertNull(calibrator.getEstimatedMyx());
5315         assertNull(calibrator.getEstimatedMyz());
5316         assertNull(calibrator.getEstimatedMzx());
5317         assertNull(calibrator.getEstimatedMzy());
5318         assertNull(calibrator.getEstimatedCovariance());
5319         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5320         assertNull(calibrator.getGroundTruthGravityNorm());
5321         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5322         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5323     }
5324 
5325     @Test
5326     public void testConstructor50() throws WrongSizeException {
5327 
5328         final Collection<StandardDeviationBodyKinematics> measurements =
5329                 Collections.emptyList();
5330 
5331         final Matrix ba = generateBa();
5332         final double biasX = ba.getElementAtIndex(0);
5333         final double biasY = ba.getElementAtIndex(1);
5334         final double biasZ = ba.getElementAtIndex(2);
5335 
5336         final Matrix ma = generateMaCommonAxis();
5337         final double sx = ma.getElementAt(0, 0);
5338         final double sy = ma.getElementAt(1, 1);
5339         final double sz = ma.getElementAt(2, 2);
5340         final double mxy = ma.getElementAt(0, 1);
5341         final double mxz = ma.getElementAt(0, 2);
5342         final double myx = ma.getElementAt(1, 0);
5343         final double myz = ma.getElementAt(1, 2);
5344         final double mzx = ma.getElementAt(2, 0);
5345         final double mzy = ma.getElementAt(2, 1);
5346 
5347         final Acceleration bx = new Acceleration(biasX,
5348                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5349         final Acceleration by = new Acceleration(biasY,
5350                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5351         final Acceleration bz = new Acceleration(biasZ,
5352                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5353 
5354         final KnownGravityNormAccelerometerCalibrator calibrator =
5355                 new KnownGravityNormAccelerometerCalibrator(
5356                         measurements, bx, by, bz, sx, sy, sz,
5357                         mxy, mxz, myx, myz, mzx, mzy, this);
5358 
5359         // check default values
5360         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5361         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5362         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5363         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5364         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5365         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5366         final Acceleration bx2 = new Acceleration(0.0,
5367                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5368         calibrator.getInitialBiasXAsAcceleration(bx2);
5369         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5370         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5371         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5372         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5373         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5374         final Acceleration by2 = new Acceleration(0.0,
5375                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5376         calibrator.getInitialBiasYAsAcceleration(by2);
5377         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5378         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5379         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5380         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5381         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5382         final Acceleration bz2 = new Acceleration(0.0,
5383                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5384         calibrator.getInitialBiasZAsAcceleration(bz2);
5385         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5386         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5387         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5388         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5389         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5390         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5391         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5392         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5393         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5394         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5395         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5396         final double[] bias1 = calibrator.getInitialBias();
5397         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5398         final double[] bias2 = new double[3];
5399         calibrator.getInitialBias(bias2);
5400         assertArrayEquals(bias1, bias2, 0.0);
5401         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5402         assertEquals(b1, ba);
5403         final Matrix b2 = new Matrix(3, 1);
5404         calibrator.getInitialBiasAsMatrix(b2);
5405         assertEquals(b1, b2);
5406         final Matrix ma1 = new Matrix(3, 3);
5407         ma1.setSubmatrix(0, 0,
5408                 2, 2,
5409                 new double[]{sx, myx, mzx,
5410                         mxy, sy, mzy,
5411                         mxz, myz, sz});
5412         assertEquals(calibrator.getInitialMa(), ma1);
5413         final Matrix ma2 = new Matrix(3, 3);
5414         calibrator.getInitialMa(ma2);
5415         assertEquals(ma1, ma2);
5416         assertSame(calibrator.getMeasurements(), measurements);
5417         assertFalse(calibrator.isCommonAxisUsed());
5418         assertSame(calibrator.getListener(), this);
5419         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5420         assertFalse(calibrator.isReady());
5421         assertFalse(calibrator.isRunning());
5422         assertNull(calibrator.getEstimatedBiases());
5423         assertFalse(calibrator.getEstimatedBiases(null));
5424         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5425         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5426         assertNull(calibrator.getEstimatedBiasFx());
5427         assertNull(calibrator.getEstimatedBiasFy());
5428         assertNull(calibrator.getEstimatedBiasFz());
5429         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5430         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5431         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5432         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5433         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5434         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5435         assertNull(calibrator.getEstimatedMa());
5436         assertNull(calibrator.getEstimatedSx());
5437         assertNull(calibrator.getEstimatedSy());
5438         assertNull(calibrator.getEstimatedSz());
5439         assertNull(calibrator.getEstimatedMxy());
5440         assertNull(calibrator.getEstimatedMxz());
5441         assertNull(calibrator.getEstimatedMyx());
5442         assertNull(calibrator.getEstimatedMyz());
5443         assertNull(calibrator.getEstimatedMzx());
5444         assertNull(calibrator.getEstimatedMzy());
5445         assertNull(calibrator.getEstimatedCovariance());
5446         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5447         assertNull(calibrator.getGroundTruthGravityNorm());
5448         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5449         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5450     }
5451 
5452     @Test
5453     public void testConstructor51() throws WrongSizeException {
5454         final Matrix ba = generateBa();
5455         final double biasX = ba.getElementAtIndex(0);
5456         final double biasY = ba.getElementAtIndex(1);
5457         final double biasZ = ba.getElementAtIndex(2);
5458 
5459         final Matrix ma = generateMaCommonAxis();
5460         final double sx = ma.getElementAt(0, 0);
5461         final double sy = ma.getElementAt(1, 1);
5462         final double sz = ma.getElementAt(2, 2);
5463         final double mxy = ma.getElementAt(0, 1);
5464         final double mxz = ma.getElementAt(0, 2);
5465         final double myx = ma.getElementAt(1, 0);
5466         final double myz = ma.getElementAt(1, 2);
5467         final double mzx = ma.getElementAt(2, 0);
5468         final double mzy = ma.getElementAt(2, 1);
5469 
5470         final Acceleration bx = new Acceleration(biasX,
5471                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5472         final Acceleration by = new Acceleration(biasY,
5473                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5474         final Acceleration bz = new Acceleration(biasZ,
5475                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5476 
5477         final KnownGravityNormAccelerometerCalibrator calibrator =
5478                 new KnownGravityNormAccelerometerCalibrator(
5479                         true, bx, by, bz, sx, sy, sz,
5480                         mxy, mxz, myx, myz, mzx, mzy);
5481 
5482         // check default values
5483         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5484         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5485         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5486         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5487         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5488         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5489         final Acceleration bx2 = new Acceleration(0.0,
5490                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5491         calibrator.getInitialBiasXAsAcceleration(bx2);
5492         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5493         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5494         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5495         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5496         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5497         final Acceleration by2 = new Acceleration(0.0,
5498                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5499         calibrator.getInitialBiasYAsAcceleration(by2);
5500         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5501         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5502         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5503         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5504         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5505         final Acceleration bz2 = new Acceleration(0.0,
5506                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5507         calibrator.getInitialBiasZAsAcceleration(bz2);
5508         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5509         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5510         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5511         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5512         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5513         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5514         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5515         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5516         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5517         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5518         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5519         final double[] bias1 = calibrator.getInitialBias();
5520         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5521         final double[] bias2 = new double[3];
5522         calibrator.getInitialBias(bias2);
5523         assertArrayEquals(bias1, bias2, 0.0);
5524         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5525         assertEquals(b1, ba);
5526         final Matrix b2 = new Matrix(3, 1);
5527         calibrator.getInitialBiasAsMatrix(b2);
5528         assertEquals(b1, b2);
5529         final Matrix ma1 = new Matrix(3, 3);
5530         ma1.setSubmatrix(0, 0,
5531                 2, 2,
5532                 new double[]{sx, myx, mzx,
5533                         mxy, sy, mzy,
5534                         mxz, myz, sz});
5535         assertEquals(calibrator.getInitialMa(), ma1);
5536         final Matrix ma2 = new Matrix(3, 3);
5537         calibrator.getInitialMa(ma2);
5538         assertEquals(ma1, ma2);
5539         assertNull(calibrator.getMeasurements());
5540         assertTrue(calibrator.isCommonAxisUsed());
5541         assertNull(calibrator.getListener());
5542         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5543         assertFalse(calibrator.isReady());
5544         assertFalse(calibrator.isRunning());
5545         assertNull(calibrator.getEstimatedBiases());
5546         assertFalse(calibrator.getEstimatedBiases(null));
5547         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5548         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5549         assertNull(calibrator.getEstimatedBiasFx());
5550         assertNull(calibrator.getEstimatedBiasFy());
5551         assertNull(calibrator.getEstimatedBiasFz());
5552         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5553         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5554         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5555         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5556         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5557         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5558         assertNull(calibrator.getEstimatedMa());
5559         assertNull(calibrator.getEstimatedSx());
5560         assertNull(calibrator.getEstimatedSy());
5561         assertNull(calibrator.getEstimatedSz());
5562         assertNull(calibrator.getEstimatedMxy());
5563         assertNull(calibrator.getEstimatedMxz());
5564         assertNull(calibrator.getEstimatedMyx());
5565         assertNull(calibrator.getEstimatedMyz());
5566         assertNull(calibrator.getEstimatedMzx());
5567         assertNull(calibrator.getEstimatedMzy());
5568         assertNull(calibrator.getEstimatedCovariance());
5569         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5570         assertNull(calibrator.getGroundTruthGravityNorm());
5571         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5572         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5573     }
5574 
5575     @Test
5576     public void testConstructor52() throws WrongSizeException {
5577         final Matrix ba = generateBa();
5578         final double biasX = ba.getElementAtIndex(0);
5579         final double biasY = ba.getElementAtIndex(1);
5580         final double biasZ = ba.getElementAtIndex(2);
5581 
5582         final Matrix ma = generateMaCommonAxis();
5583         final double sx = ma.getElementAt(0, 0);
5584         final double sy = ma.getElementAt(1, 1);
5585         final double sz = ma.getElementAt(2, 2);
5586         final double mxy = ma.getElementAt(0, 1);
5587         final double mxz = ma.getElementAt(0, 2);
5588         final double myx = ma.getElementAt(1, 0);
5589         final double myz = ma.getElementAt(1, 2);
5590         final double mzx = ma.getElementAt(2, 0);
5591         final double mzy = ma.getElementAt(2, 1);
5592 
5593         final Acceleration bx = new Acceleration(biasX,
5594                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5595         final Acceleration by = new Acceleration(biasY,
5596                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5597         final Acceleration bz = new Acceleration(biasZ,
5598                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5599 
5600         final KnownGravityNormAccelerometerCalibrator calibrator =
5601                 new KnownGravityNormAccelerometerCalibrator(true,
5602                         bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
5603                         this);
5604 
5605         // check default values
5606         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5607         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5608         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5609         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5610         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5611         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5612         final Acceleration bx2 = new Acceleration(0.0,
5613                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5614         calibrator.getInitialBiasXAsAcceleration(bx2);
5615         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5616         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5617         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5618         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5619         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5620         final Acceleration by2 = new Acceleration(0.0,
5621                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5622         calibrator.getInitialBiasYAsAcceleration(by2);
5623         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5624         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5625         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5626         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5627         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5628         final Acceleration bz2 = new Acceleration(0.0,
5629                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5630         calibrator.getInitialBiasZAsAcceleration(bz2);
5631         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5632         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5633         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5634         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5635         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5636         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5637         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5638         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5639         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5640         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5641         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5642         final double[] bias1 = calibrator.getInitialBias();
5643         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5644         final double[] bias2 = new double[3];
5645         calibrator.getInitialBias(bias2);
5646         assertArrayEquals(bias1, bias2, 0.0);
5647         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5648         assertEquals(b1, ba);
5649         final Matrix b2 = new Matrix(3, 1);
5650         calibrator.getInitialBiasAsMatrix(b2);
5651         assertEquals(b1, b2);
5652         final Matrix ma1 = new Matrix(3, 3);
5653         ma1.setSubmatrix(0, 0,
5654                 2, 2,
5655                 new double[]{sx, myx, mzx,
5656                         mxy, sy, mzy,
5657                         mxz, myz, sz});
5658         assertEquals(calibrator.getInitialMa(), ma1);
5659         final Matrix ma2 = new Matrix(3, 3);
5660         calibrator.getInitialMa(ma2);
5661         assertEquals(ma1, ma2);
5662         assertNull(calibrator.getMeasurements());
5663         assertTrue(calibrator.isCommonAxisUsed());
5664         assertSame(calibrator.getListener(), this);
5665         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5666         assertFalse(calibrator.isReady());
5667         assertFalse(calibrator.isRunning());
5668         assertNull(calibrator.getEstimatedBiases());
5669         assertFalse(calibrator.getEstimatedBiases(null));
5670         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5671         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5672         assertNull(calibrator.getEstimatedBiasFx());
5673         assertNull(calibrator.getEstimatedBiasFy());
5674         assertNull(calibrator.getEstimatedBiasFz());
5675         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5676         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5677         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5678         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5679         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5680         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5681         assertNull(calibrator.getEstimatedMa());
5682         assertNull(calibrator.getEstimatedSx());
5683         assertNull(calibrator.getEstimatedSy());
5684         assertNull(calibrator.getEstimatedSz());
5685         assertNull(calibrator.getEstimatedMxy());
5686         assertNull(calibrator.getEstimatedMxz());
5687         assertNull(calibrator.getEstimatedMyx());
5688         assertNull(calibrator.getEstimatedMyz());
5689         assertNull(calibrator.getEstimatedMzx());
5690         assertNull(calibrator.getEstimatedMzy());
5691         assertNull(calibrator.getEstimatedCovariance());
5692         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5693         assertNull(calibrator.getGroundTruthGravityNorm());
5694         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5695         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5696     }
5697 
5698     @Test
5699     public void testConstructor53() throws WrongSizeException {
5700         final Collection<StandardDeviationBodyKinematics> measurements =
5701                 Collections.emptyList();
5702 
5703         final Matrix ba = generateBa();
5704         final double biasX = ba.getElementAtIndex(0);
5705         final double biasY = ba.getElementAtIndex(1);
5706         final double biasZ = ba.getElementAtIndex(2);
5707 
5708         final Matrix ma = generateMaCommonAxis();
5709         final double sx = ma.getElementAt(0, 0);
5710         final double sy = ma.getElementAt(1, 1);
5711         final double sz = ma.getElementAt(2, 2);
5712         final double mxy = ma.getElementAt(0, 1);
5713         final double mxz = ma.getElementAt(0, 2);
5714         final double myx = ma.getElementAt(1, 0);
5715         final double myz = ma.getElementAt(1, 2);
5716         final double mzx = ma.getElementAt(2, 0);
5717         final double mzy = ma.getElementAt(2, 1);
5718 
5719         final Acceleration bx = new Acceleration(biasX,
5720                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5721         final Acceleration by = new Acceleration(biasY,
5722                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5723         final Acceleration bz = new Acceleration(biasZ,
5724                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5725 
5726         final KnownGravityNormAccelerometerCalibrator calibrator =
5727                 new KnownGravityNormAccelerometerCalibrator(measurements,
5728                         true, bx, by, bz, sx, sy, sz,
5729                         mxy, mxz, myx, myz, mzx, mzy);
5730 
5731         // check default values
5732         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5733         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5734         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5735         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5736         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5737         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5738         final Acceleration bx2 = new Acceleration(0.0,
5739                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5740         calibrator.getInitialBiasXAsAcceleration(bx2);
5741         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5742         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5743         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5744         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5745         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5746         final Acceleration by2 = new Acceleration(0.0,
5747                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5748         calibrator.getInitialBiasYAsAcceleration(by2);
5749         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5750         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5751         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5752         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5753         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5754         final Acceleration bz2 = new Acceleration(0.0,
5755                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5756         calibrator.getInitialBiasZAsAcceleration(bz2);
5757         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5758         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5759         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5760         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5761         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5762         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5763         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5764         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5765         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5766         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5767         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5768         final double[] bias1 = calibrator.getInitialBias();
5769         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5770         final double[] bias2 = new double[3];
5771         calibrator.getInitialBias(bias2);
5772         assertArrayEquals(bias1, bias2, 0.0);
5773         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5774         assertEquals(b1, ba);
5775         final Matrix b2 = new Matrix(3, 1);
5776         calibrator.getInitialBiasAsMatrix(b2);
5777         assertEquals(b1, b2);
5778         final Matrix ma1 = new Matrix(3, 3);
5779         ma1.setSubmatrix(0, 0,
5780                 2, 2,
5781                 new double[]{sx, myx, mzx,
5782                         mxy, sy, mzy,
5783                         mxz, myz, sz});
5784         assertEquals(calibrator.getInitialMa(), ma1);
5785         final Matrix ma2 = new Matrix(3, 3);
5786         calibrator.getInitialMa(ma2);
5787         assertEquals(ma1, ma2);
5788         assertSame(calibrator.getMeasurements(), measurements);
5789         assertTrue(calibrator.isCommonAxisUsed());
5790         assertNull(calibrator.getListener());
5791         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5792         assertFalse(calibrator.isReady());
5793         assertFalse(calibrator.isRunning());
5794         assertNull(calibrator.getEstimatedBiases());
5795         assertFalse(calibrator.getEstimatedBiases(null));
5796         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5797         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5798         assertNull(calibrator.getEstimatedBiasFx());
5799         assertNull(calibrator.getEstimatedBiasFy());
5800         assertNull(calibrator.getEstimatedBiasFz());
5801         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5802         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5803         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5804         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5805         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5806         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5807         assertNull(calibrator.getEstimatedMa());
5808         assertNull(calibrator.getEstimatedSx());
5809         assertNull(calibrator.getEstimatedSy());
5810         assertNull(calibrator.getEstimatedSz());
5811         assertNull(calibrator.getEstimatedMxy());
5812         assertNull(calibrator.getEstimatedMxz());
5813         assertNull(calibrator.getEstimatedMyx());
5814         assertNull(calibrator.getEstimatedMyz());
5815         assertNull(calibrator.getEstimatedMzx());
5816         assertNull(calibrator.getEstimatedMzy());
5817         assertNull(calibrator.getEstimatedCovariance());
5818         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5819         assertNull(calibrator.getGroundTruthGravityNorm());
5820         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5821         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5822     }
5823 
5824     @Test
5825     public void testConstructor54() throws WrongSizeException {
5826         final Collection<StandardDeviationBodyKinematics> measurements =
5827                 Collections.emptyList();
5828 
5829         final Matrix ba = generateBa();
5830         final double biasX = ba.getElementAtIndex(0);
5831         final double biasY = ba.getElementAtIndex(1);
5832         final double biasZ = ba.getElementAtIndex(2);
5833 
5834         final Matrix ma = generateMaCommonAxis();
5835         final double sx = ma.getElementAt(0, 0);
5836         final double sy = ma.getElementAt(1, 1);
5837         final double sz = ma.getElementAt(2, 2);
5838         final double mxy = ma.getElementAt(0, 1);
5839         final double mxz = ma.getElementAt(0, 2);
5840         final double myx = ma.getElementAt(1, 0);
5841         final double myz = ma.getElementAt(1, 2);
5842         final double mzx = ma.getElementAt(2, 0);
5843         final double mzy = ma.getElementAt(2, 1);
5844 
5845         final Acceleration bx = new Acceleration(biasX,
5846                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5847         final Acceleration by = new Acceleration(biasY,
5848                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5849         final Acceleration bz = new Acceleration(biasZ,
5850                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5851 
5852         final KnownGravityNormAccelerometerCalibrator calibrator =
5853                 new KnownGravityNormAccelerometerCalibrator(measurements,
5854                         true, bx, by, bz, sx, sy, sz,
5855                         mxy, mxz, myx, myz, mzx, mzy, this);
5856 
5857         // check default values
5858         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5859         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5860         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5861         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5862         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5863         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5864         final Acceleration bx2 = new Acceleration(0.0,
5865                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5866         calibrator.getInitialBiasXAsAcceleration(bx2);
5867         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5868         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5869         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5870         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5871         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5872         final Acceleration by2 = new Acceleration(0.0,
5873                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5874         calibrator.getInitialBiasYAsAcceleration(by2);
5875         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5876         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5877         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5878         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5879         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5880         final Acceleration bz2 = new Acceleration(0.0,
5881                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5882         calibrator.getInitialBiasZAsAcceleration(bz2);
5883         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5884         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5885         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5886         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5887         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5888         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5889         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5890         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5891         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5892         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5893         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5894         final double[] bias1 = calibrator.getInitialBias();
5895         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5896         final double[] bias2 = new double[3];
5897         calibrator.getInitialBias(bias2);
5898         assertArrayEquals(bias1, bias2, 0.0);
5899         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5900         assertEquals(b1, ba);
5901         final Matrix b2 = new Matrix(3, 1);
5902         calibrator.getInitialBiasAsMatrix(b2);
5903         assertEquals(b1, b2);
5904         final Matrix ma1 = new Matrix(3, 3);
5905         ma1.setSubmatrix(0, 0,
5906                 2, 2,
5907                 new double[]{sx, myx, mzx,
5908                         mxy, sy, mzy,
5909                         mxz, myz, sz});
5910         assertEquals(calibrator.getInitialMa(), ma1);
5911         final Matrix ma2 = new Matrix(3, 3);
5912         calibrator.getInitialMa(ma2);
5913         assertEquals(ma1, ma2);
5914         assertSame(calibrator.getMeasurements(), measurements);
5915         assertTrue(calibrator.isCommonAxisUsed());
5916         assertSame(calibrator.getListener(), this);
5917         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5918         assertFalse(calibrator.isReady());
5919         assertFalse(calibrator.isRunning());
5920         assertNull(calibrator.getEstimatedBiases());
5921         assertFalse(calibrator.getEstimatedBiases(null));
5922         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5923         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5924         assertNull(calibrator.getEstimatedBiasFx());
5925         assertNull(calibrator.getEstimatedBiasFy());
5926         assertNull(calibrator.getEstimatedBiasFz());
5927         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5928         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5929         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5930         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5931         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5932         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5933         assertNull(calibrator.getEstimatedMa());
5934         assertNull(calibrator.getEstimatedSx());
5935         assertNull(calibrator.getEstimatedSy());
5936         assertNull(calibrator.getEstimatedSz());
5937         assertNull(calibrator.getEstimatedMxy());
5938         assertNull(calibrator.getEstimatedMxz());
5939         assertNull(calibrator.getEstimatedMyx());
5940         assertNull(calibrator.getEstimatedMyz());
5941         assertNull(calibrator.getEstimatedMzx());
5942         assertNull(calibrator.getEstimatedMzy());
5943         assertNull(calibrator.getEstimatedCovariance());
5944         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5945         assertNull(calibrator.getGroundTruthGravityNorm());
5946         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5947         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5948     }
5949 
5950     @Test
5951     public void testConstructor55() throws WrongSizeException {
5952         final Matrix ba = generateBa();
5953         final double[] bias = ba.getBuffer();
5954         final double biasX = ba.getElementAtIndex(0);
5955         final double biasY = ba.getElementAtIndex(1);
5956         final double biasZ = ba.getElementAtIndex(2);
5957 
5958         KnownGravityNormAccelerometerCalibrator calibrator =
5959                 new KnownGravityNormAccelerometerCalibrator(bias);
5960 
5961         // check default values
5962         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5963         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5964         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5965         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5966         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5967         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5968         final Acceleration bx2 = new Acceleration(0.0,
5969                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5970         calibrator.getInitialBiasXAsAcceleration(bx2);
5971         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5972         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5973         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5974         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5975         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5976         final Acceleration by2 = new Acceleration(0.0,
5977                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5978         calibrator.getInitialBiasYAsAcceleration(by2);
5979         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5980         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5981         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5982         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5983         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5984         final Acceleration bz2 = new Acceleration(0.0,
5985                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5986         calibrator.getInitialBiasZAsAcceleration(bz2);
5987         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5988         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5989         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
5990         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
5991         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
5992         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
5993         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
5994         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
5995         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
5996         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
5997         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
5998         final double[] bias1 = calibrator.getInitialBias();
5999         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6000         final double[] bias2 = new double[3];
6001         calibrator.getInitialBias(bias2);
6002         assertArrayEquals(bias1, bias2, 0.0);
6003         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6004         assertEquals(b1, ba);
6005         final Matrix b2 = new Matrix(3, 1);
6006         calibrator.getInitialBiasAsMatrix(b2);
6007         assertEquals(b1, b2);
6008         final Matrix ma1 = calibrator.getInitialMa();
6009         assertEquals(ma1, new Matrix(3, 3));
6010         final Matrix ma2 = new Matrix(3, 3);
6011         calibrator.getInitialMa(ma2);
6012         assertEquals(ma1, ma2);
6013         assertNull(calibrator.getMeasurements());
6014         assertFalse(calibrator.isCommonAxisUsed());
6015         assertNull(calibrator.getListener());
6016         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6017         assertFalse(calibrator.isReady());
6018         assertFalse(calibrator.isRunning());
6019         assertNull(calibrator.getEstimatedBiases());
6020         assertFalse(calibrator.getEstimatedBiases(null));
6021         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6022         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6023         assertNull(calibrator.getEstimatedBiasFx());
6024         assertNull(calibrator.getEstimatedBiasFy());
6025         assertNull(calibrator.getEstimatedBiasFz());
6026         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6027         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6028         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6029         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6030         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6031         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6032         assertNull(calibrator.getEstimatedMa());
6033         assertNull(calibrator.getEstimatedSx());
6034         assertNull(calibrator.getEstimatedSy());
6035         assertNull(calibrator.getEstimatedSz());
6036         assertNull(calibrator.getEstimatedMxy());
6037         assertNull(calibrator.getEstimatedMxz());
6038         assertNull(calibrator.getEstimatedMyx());
6039         assertNull(calibrator.getEstimatedMyz());
6040         assertNull(calibrator.getEstimatedMzx());
6041         assertNull(calibrator.getEstimatedMzy());
6042         assertNull(calibrator.getEstimatedCovariance());
6043         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6044         assertNull(calibrator.getGroundTruthGravityNorm());
6045         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6046         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6047 
6048         // Force IllegalArgumentException
6049         calibrator = null;
6050         try {
6051             calibrator = new KnownGravityNormAccelerometerCalibrator(
6052                     new double[1]);
6053             fail("IllegalArgumentException expected but not thrown");
6054         } catch (final IllegalArgumentException ignore) {
6055         }
6056         assertNull(calibrator);
6057     }
6058 
6059     @Test
6060     public void testConstructor56() throws WrongSizeException {
6061         final Matrix ba = generateBa();
6062         final double[] bias = ba.getBuffer();
6063         final double biasX = ba.getElementAtIndex(0);
6064         final double biasY = ba.getElementAtIndex(1);
6065         final double biasZ = ba.getElementAtIndex(2);
6066 
6067         KnownGravityNormAccelerometerCalibrator calibrator =
6068                 new KnownGravityNormAccelerometerCalibrator(bias,
6069                         this);
6070 
6071         // check default values
6072         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6073         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6074         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6075         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6076         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6077         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6078         final Acceleration bx2 = new Acceleration(0.0,
6079                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6080         calibrator.getInitialBiasXAsAcceleration(bx2);
6081         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6082         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6083         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6084         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6085         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6086         final Acceleration by2 = new Acceleration(0.0,
6087                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6088         calibrator.getInitialBiasYAsAcceleration(by2);
6089         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6090         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6091         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6092         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6093         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6094         final Acceleration bz2 = new Acceleration(0.0,
6095                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6096         calibrator.getInitialBiasZAsAcceleration(bz2);
6097         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6098         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6099         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6100         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6101         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6102         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6103         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6104         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6105         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6106         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6107         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6108         final double[] bias1 = calibrator.getInitialBias();
6109         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6110         final double[] bias2 = new double[3];
6111         calibrator.getInitialBias(bias2);
6112         assertArrayEquals(bias1, bias2, 0.0);
6113         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6114         assertEquals(b1, ba);
6115         final Matrix b2 = new Matrix(3, 1);
6116         calibrator.getInitialBiasAsMatrix(b2);
6117         assertEquals(b1, b2);
6118         final Matrix ma1 = calibrator.getInitialMa();
6119         assertEquals(ma1, new Matrix(3, 3));
6120         final Matrix ma2 = new Matrix(3, 3);
6121         calibrator.getInitialMa(ma2);
6122         assertEquals(ma1, ma2);
6123         assertNull(calibrator.getMeasurements());
6124         assertFalse(calibrator.isCommonAxisUsed());
6125         assertSame(calibrator.getListener(), this);
6126         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6127         assertFalse(calibrator.isReady());
6128         assertFalse(calibrator.isRunning());
6129         assertNull(calibrator.getEstimatedBiases());
6130         assertFalse(calibrator.getEstimatedBiases(null));
6131         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6132         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6133         assertNull(calibrator.getEstimatedBiasFx());
6134         assertNull(calibrator.getEstimatedBiasFy());
6135         assertNull(calibrator.getEstimatedBiasFz());
6136         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6137         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6138         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6139         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6140         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6141         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6142         assertNull(calibrator.getEstimatedMa());
6143         assertNull(calibrator.getEstimatedSx());
6144         assertNull(calibrator.getEstimatedSy());
6145         assertNull(calibrator.getEstimatedSz());
6146         assertNull(calibrator.getEstimatedMxy());
6147         assertNull(calibrator.getEstimatedMxz());
6148         assertNull(calibrator.getEstimatedMyx());
6149         assertNull(calibrator.getEstimatedMyz());
6150         assertNull(calibrator.getEstimatedMzx());
6151         assertNull(calibrator.getEstimatedMzy());
6152         assertNull(calibrator.getEstimatedCovariance());
6153         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6154         assertNull(calibrator.getGroundTruthGravityNorm());
6155         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6156         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6157 
6158         // Force IllegalArgumentException
6159         calibrator = null;
6160         try {
6161             calibrator = new KnownGravityNormAccelerometerCalibrator(
6162                     new double[1], this);
6163             fail("IllegalArgumentException expected but not thrown");
6164         } catch (final IllegalArgumentException ignore) {
6165         }
6166         assertNull(calibrator);
6167     }
6168 
6169     @Test
6170     public void testConstructor57() throws WrongSizeException {
6171         final Collection<StandardDeviationBodyKinematics> measurements =
6172                 Collections.emptyList();
6173 
6174         final Matrix ba = generateBa();
6175         final double[] bias = ba.getBuffer();
6176         final double biasX = ba.getElementAtIndex(0);
6177         final double biasY = ba.getElementAtIndex(1);
6178         final double biasZ = ba.getElementAtIndex(2);
6179 
6180         KnownGravityNormAccelerometerCalibrator calibrator =
6181                 new KnownGravityNormAccelerometerCalibrator(
6182                         measurements, bias);
6183 
6184         // check default values
6185         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6186         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6187         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6188         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6189         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6190         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6191         final Acceleration bx2 = new Acceleration(0.0,
6192                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6193         calibrator.getInitialBiasXAsAcceleration(bx2);
6194         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6195         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6196         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6197         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6198         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6199         final Acceleration by2 = new Acceleration(0.0,
6200                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6201         calibrator.getInitialBiasYAsAcceleration(by2);
6202         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6203         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6204         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6205         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6206         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6207         final Acceleration bz2 = new Acceleration(0.0,
6208                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6209         calibrator.getInitialBiasZAsAcceleration(bz2);
6210         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6211         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6212         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6213         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6214         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6215         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6216         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6217         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6218         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6219         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6220         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6221         final double[] bias1 = calibrator.getInitialBias();
6222         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6223         final double[] bias2 = new double[3];
6224         calibrator.getInitialBias(bias2);
6225         assertArrayEquals(bias1, bias2, 0.0);
6226         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6227         assertEquals(b1, ba);
6228         final Matrix b2 = new Matrix(3, 1);
6229         calibrator.getInitialBiasAsMatrix(b2);
6230         assertEquals(b1, b2);
6231         final Matrix ma1 = calibrator.getInitialMa();
6232         assertEquals(ma1, new Matrix(3, 3));
6233         final Matrix ma2 = new Matrix(3, 3);
6234         calibrator.getInitialMa(ma2);
6235         assertEquals(ma1, ma2);
6236         assertSame(calibrator.getMeasurements(), measurements);
6237         assertFalse(calibrator.isCommonAxisUsed());
6238         assertNull(calibrator.getListener());
6239         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6240         assertFalse(calibrator.isReady());
6241         assertFalse(calibrator.isRunning());
6242         assertNull(calibrator.getEstimatedBiases());
6243         assertFalse(calibrator.getEstimatedBiases(null));
6244         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6245         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6246         assertNull(calibrator.getEstimatedBiasFx());
6247         assertNull(calibrator.getEstimatedBiasFy());
6248         assertNull(calibrator.getEstimatedBiasFz());
6249         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6250         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6251         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6252         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6253         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6254         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6255         assertNull(calibrator.getEstimatedMa());
6256         assertNull(calibrator.getEstimatedSx());
6257         assertNull(calibrator.getEstimatedSy());
6258         assertNull(calibrator.getEstimatedSz());
6259         assertNull(calibrator.getEstimatedMxy());
6260         assertNull(calibrator.getEstimatedMxz());
6261         assertNull(calibrator.getEstimatedMyx());
6262         assertNull(calibrator.getEstimatedMyz());
6263         assertNull(calibrator.getEstimatedMzx());
6264         assertNull(calibrator.getEstimatedMzy());
6265         assertNull(calibrator.getEstimatedCovariance());
6266         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6267         assertNull(calibrator.getGroundTruthGravityNorm());
6268         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6269         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6270 
6271         // Force IllegalArgumentException
6272         calibrator = null;
6273         try {
6274             calibrator = new KnownGravityNormAccelerometerCalibrator(
6275                     measurements, new double[1]);
6276             fail("IllegalArgumentException expected but not thrown");
6277         } catch (final IllegalArgumentException ignore) {
6278         }
6279         assertNull(calibrator);
6280     }
6281 
6282     @Test
6283     public void testConstructor58() throws WrongSizeException {
6284         final Collection<StandardDeviationBodyKinematics> measurements =
6285                 Collections.emptyList();
6286 
6287         final Matrix ba = generateBa();
6288         final double[] bias = ba.getBuffer();
6289         final double biasX = ba.getElementAtIndex(0);
6290         final double biasY = ba.getElementAtIndex(1);
6291         final double biasZ = ba.getElementAtIndex(2);
6292 
6293         KnownGravityNormAccelerometerCalibrator calibrator =
6294                 new KnownGravityNormAccelerometerCalibrator(
6295                         measurements, bias, this);
6296 
6297         // check default values
6298         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6299         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6300         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6301         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6302         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6303         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6304         final Acceleration bx2 = new Acceleration(0.0,
6305                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6306         calibrator.getInitialBiasXAsAcceleration(bx2);
6307         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6308         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6309         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6310         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6311         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6312         final Acceleration by2 = new Acceleration(0.0,
6313                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6314         calibrator.getInitialBiasYAsAcceleration(by2);
6315         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6316         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6317         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6318         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6319         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6320         final Acceleration bz2 = new Acceleration(0.0,
6321                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6322         calibrator.getInitialBiasZAsAcceleration(bz2);
6323         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6324         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6325         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6326         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6327         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6328         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6329         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6330         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6331         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6332         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6333         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6334         final double[] bias1 = calibrator.getInitialBias();
6335         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6336         final double[] bias2 = new double[3];
6337         calibrator.getInitialBias(bias2);
6338         assertArrayEquals(bias1, bias2, 0.0);
6339         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6340         assertEquals(b1, ba);
6341         final Matrix b2 = new Matrix(3, 1);
6342         calibrator.getInitialBiasAsMatrix(b2);
6343         assertEquals(b1, b2);
6344         final Matrix ma1 = calibrator.getInitialMa();
6345         assertEquals(ma1, new Matrix(3, 3));
6346         final Matrix ma2 = new Matrix(3, 3);
6347         calibrator.getInitialMa(ma2);
6348         assertEquals(ma1, ma2);
6349         assertSame(calibrator.getMeasurements(), measurements);
6350         assertFalse(calibrator.isCommonAxisUsed());
6351         assertSame(calibrator.getListener(), this);
6352         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6353         assertFalse(calibrator.isReady());
6354         assertFalse(calibrator.isRunning());
6355         assertNull(calibrator.getEstimatedBiases());
6356         assertFalse(calibrator.getEstimatedBiases(null));
6357         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6358         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6359         assertNull(calibrator.getEstimatedBiasFx());
6360         assertNull(calibrator.getEstimatedBiasFy());
6361         assertNull(calibrator.getEstimatedBiasFz());
6362         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6363         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6364         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6365         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6366         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6367         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6368         assertNull(calibrator.getEstimatedMa());
6369         assertNull(calibrator.getEstimatedSx());
6370         assertNull(calibrator.getEstimatedSy());
6371         assertNull(calibrator.getEstimatedSz());
6372         assertNull(calibrator.getEstimatedMxy());
6373         assertNull(calibrator.getEstimatedMxz());
6374         assertNull(calibrator.getEstimatedMyx());
6375         assertNull(calibrator.getEstimatedMyz());
6376         assertNull(calibrator.getEstimatedMzx());
6377         assertNull(calibrator.getEstimatedMzy());
6378         assertNull(calibrator.getEstimatedCovariance());
6379         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6380         assertNull(calibrator.getGroundTruthGravityNorm());
6381         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6382         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6383 
6384         // Force IllegalArgumentException
6385         calibrator = null;
6386         try {
6387             calibrator = new KnownGravityNormAccelerometerCalibrator(
6388                     measurements, new double[1], this);
6389             fail("IllegalArgumentException expected but not thrown");
6390         } catch (final IllegalArgumentException ignore) {
6391         }
6392         assertNull(calibrator);
6393     }
6394 
6395     @Test
6396     public void testConstructor59() throws WrongSizeException {
6397         final Matrix ba = generateBa();
6398         final double[] bias = ba.getBuffer();
6399         final double biasX = ba.getElementAtIndex(0);
6400         final double biasY = ba.getElementAtIndex(1);
6401         final double biasZ = ba.getElementAtIndex(2);
6402 
6403         KnownGravityNormAccelerometerCalibrator calibrator =
6404                 new KnownGravityNormAccelerometerCalibrator(
6405                         true, bias);
6406 
6407         // check default values
6408         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6409         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6410         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6411         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6412         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6413         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6414         final Acceleration bx2 = new Acceleration(0.0,
6415                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6416         calibrator.getInitialBiasXAsAcceleration(bx2);
6417         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6418         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6419         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6420         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6421         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6422         final Acceleration by2 = new Acceleration(0.0,
6423                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6424         calibrator.getInitialBiasYAsAcceleration(by2);
6425         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6426         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6427         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6428         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6429         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6430         final Acceleration bz2 = new Acceleration(0.0,
6431                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6432         calibrator.getInitialBiasZAsAcceleration(bz2);
6433         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6434         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6435         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6436         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6437         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6438         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6439         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6440         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6441         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6442         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6443         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6444         final double[] bias1 = calibrator.getInitialBias();
6445         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6446         final double[] bias2 = new double[3];
6447         calibrator.getInitialBias(bias2);
6448         assertArrayEquals(bias1, bias2, 0.0);
6449         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6450         assertEquals(b1, ba);
6451         final Matrix b2 = new Matrix(3, 1);
6452         calibrator.getInitialBiasAsMatrix(b2);
6453         assertEquals(b1, b2);
6454         final Matrix ma1 = calibrator.getInitialMa();
6455         assertEquals(ma1, new Matrix(3, 3));
6456         final Matrix ma2 = new Matrix(3, 3);
6457         calibrator.getInitialMa(ma2);
6458         assertEquals(ma1, ma2);
6459         assertNull(calibrator.getMeasurements());
6460         assertTrue(calibrator.isCommonAxisUsed());
6461         assertNull(calibrator.getListener());
6462         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6463         assertFalse(calibrator.isReady());
6464         assertFalse(calibrator.isRunning());
6465         assertNull(calibrator.getEstimatedBiases());
6466         assertFalse(calibrator.getEstimatedBiases(null));
6467         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6468         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6469         assertNull(calibrator.getEstimatedBiasFx());
6470         assertNull(calibrator.getEstimatedBiasFy());
6471         assertNull(calibrator.getEstimatedBiasFz());
6472         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6473         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6474         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6475         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6476         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6477         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6478         assertNull(calibrator.getEstimatedMa());
6479         assertNull(calibrator.getEstimatedSx());
6480         assertNull(calibrator.getEstimatedSy());
6481         assertNull(calibrator.getEstimatedSz());
6482         assertNull(calibrator.getEstimatedMxy());
6483         assertNull(calibrator.getEstimatedMxz());
6484         assertNull(calibrator.getEstimatedMyx());
6485         assertNull(calibrator.getEstimatedMyz());
6486         assertNull(calibrator.getEstimatedMzx());
6487         assertNull(calibrator.getEstimatedMzy());
6488         assertNull(calibrator.getEstimatedCovariance());
6489         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6490         assertNull(calibrator.getGroundTruthGravityNorm());
6491         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6492         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6493 
6494         // Force IllegalArgumentException
6495         calibrator = null;
6496         try {
6497             calibrator = new KnownGravityNormAccelerometerCalibrator(
6498                     true, new double[1]);
6499             fail("IllegalArgumentException expected but not thrown");
6500         } catch (final IllegalArgumentException ignore) {
6501         }
6502         assertNull(calibrator);
6503     }
6504 
6505     @Test
6506     public void testConstructor60() throws WrongSizeException {
6507         final Matrix ba = generateBa();
6508         final double[] bias = ba.getBuffer();
6509         final double biasX = ba.getElementAtIndex(0);
6510         final double biasY = ba.getElementAtIndex(1);
6511         final double biasZ = ba.getElementAtIndex(2);
6512 
6513         KnownGravityNormAccelerometerCalibrator calibrator =
6514                 new KnownGravityNormAccelerometerCalibrator(
6515                         true, bias, this);
6516 
6517         // check default values
6518         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6519         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6520         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6521         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6522         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6523         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6524         final Acceleration bx2 = new Acceleration(0.0,
6525                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6526         calibrator.getInitialBiasXAsAcceleration(bx2);
6527         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6528         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6529         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6530         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6531         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6532         final Acceleration by2 = new Acceleration(0.0,
6533                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6534         calibrator.getInitialBiasYAsAcceleration(by2);
6535         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6536         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6537         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6538         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6539         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6540         final Acceleration bz2 = new Acceleration(0.0,
6541                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6542         calibrator.getInitialBiasZAsAcceleration(bz2);
6543         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6544         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6545         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6546         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6547         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6548         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6549         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6550         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6551         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6552         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6553         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6554         final double[] bias1 = calibrator.getInitialBias();
6555         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6556         final double[] bias2 = new double[3];
6557         calibrator.getInitialBias(bias2);
6558         assertArrayEquals(bias1, bias2, 0.0);
6559         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6560         assertEquals(b1, ba);
6561         final Matrix b2 = new Matrix(3, 1);
6562         calibrator.getInitialBiasAsMatrix(b2);
6563         assertEquals(b1, b2);
6564         final Matrix ma1 = calibrator.getInitialMa();
6565         assertEquals(ma1, new Matrix(3, 3));
6566         final Matrix ma2 = new Matrix(3, 3);
6567         calibrator.getInitialMa(ma2);
6568         assertEquals(ma1, ma2);
6569         assertNull(calibrator.getMeasurements());
6570         assertTrue(calibrator.isCommonAxisUsed());
6571         assertSame(calibrator.getListener(), this);
6572         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6573         assertFalse(calibrator.isReady());
6574         assertFalse(calibrator.isRunning());
6575         assertNull(calibrator.getEstimatedBiases());
6576         assertFalse(calibrator.getEstimatedBiases(null));
6577         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6578         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6579         assertNull(calibrator.getEstimatedBiasFx());
6580         assertNull(calibrator.getEstimatedBiasFy());
6581         assertNull(calibrator.getEstimatedBiasFz());
6582         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6583         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6584         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6585         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6586         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6587         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6588         assertNull(calibrator.getEstimatedMa());
6589         assertNull(calibrator.getEstimatedSx());
6590         assertNull(calibrator.getEstimatedSy());
6591         assertNull(calibrator.getEstimatedSz());
6592         assertNull(calibrator.getEstimatedMxy());
6593         assertNull(calibrator.getEstimatedMxz());
6594         assertNull(calibrator.getEstimatedMyx());
6595         assertNull(calibrator.getEstimatedMyz());
6596         assertNull(calibrator.getEstimatedMzx());
6597         assertNull(calibrator.getEstimatedMzy());
6598         assertNull(calibrator.getEstimatedCovariance());
6599         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6600         assertNull(calibrator.getGroundTruthGravityNorm());
6601         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6602         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6603 
6604         // Force IllegalArgumentException
6605         calibrator = null;
6606         try {
6607             calibrator = new KnownGravityNormAccelerometerCalibrator(
6608                     true, new double[1], this);
6609             fail("IllegalArgumentException expected but not thrown");
6610         } catch (final IllegalArgumentException ignore) {
6611         }
6612         assertNull(calibrator);
6613     }
6614 
6615     @Test
6616     public void testConstructor61() throws WrongSizeException {
6617         final Collection<StandardDeviationBodyKinematics> measurements =
6618                 Collections.emptyList();
6619 
6620         final Matrix ba = generateBa();
6621         final double[] bias = ba.getBuffer();
6622         final double biasX = ba.getElementAtIndex(0);
6623         final double biasY = ba.getElementAtIndex(1);
6624         final double biasZ = ba.getElementAtIndex(2);
6625 
6626         KnownGravityNormAccelerometerCalibrator calibrator =
6627                 new KnownGravityNormAccelerometerCalibrator(measurements,
6628                         true, bias);
6629 
6630         // check default values
6631         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6632         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6633         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6634         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6635         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6636         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6637         final Acceleration bx2 = new Acceleration(0.0,
6638                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6639         calibrator.getInitialBiasXAsAcceleration(bx2);
6640         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6641         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6642         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6643         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6644         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6645         final Acceleration by2 = new Acceleration(0.0,
6646                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6647         calibrator.getInitialBiasYAsAcceleration(by2);
6648         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6649         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6650         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6651         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6652         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6653         final Acceleration bz2 = new Acceleration(0.0,
6654                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6655         calibrator.getInitialBiasZAsAcceleration(bz2);
6656         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6657         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6658         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6659         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6660         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6661         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6662         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6663         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6664         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6665         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6666         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6667         final double[] bias1 = calibrator.getInitialBias();
6668         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6669         final double[] bias2 = new double[3];
6670         calibrator.getInitialBias(bias2);
6671         assertArrayEquals(bias1, bias2, 0.0);
6672         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6673         assertEquals(b1, ba);
6674         final Matrix b2 = new Matrix(3, 1);
6675         calibrator.getInitialBiasAsMatrix(b2);
6676         assertEquals(b1, b2);
6677         final Matrix ma1 = calibrator.getInitialMa();
6678         assertEquals(ma1, new Matrix(3, 3));
6679         final Matrix ma2 = new Matrix(3, 3);
6680         calibrator.getInitialMa(ma2);
6681         assertEquals(ma1, ma2);
6682         assertSame(calibrator.getMeasurements(), measurements);
6683         assertTrue(calibrator.isCommonAxisUsed());
6684         assertNull(calibrator.getListener());
6685         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6686         assertFalse(calibrator.isReady());
6687         assertFalse(calibrator.isRunning());
6688         assertNull(calibrator.getEstimatedBiases());
6689         assertFalse(calibrator.getEstimatedBiases(null));
6690         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6691         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6692         assertNull(calibrator.getEstimatedBiasFx());
6693         assertNull(calibrator.getEstimatedBiasFy());
6694         assertNull(calibrator.getEstimatedBiasFz());
6695         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6696         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6697         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6698         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6699         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6700         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6701         assertNull(calibrator.getEstimatedMa());
6702         assertNull(calibrator.getEstimatedSx());
6703         assertNull(calibrator.getEstimatedSy());
6704         assertNull(calibrator.getEstimatedSz());
6705         assertNull(calibrator.getEstimatedMxy());
6706         assertNull(calibrator.getEstimatedMxz());
6707         assertNull(calibrator.getEstimatedMyx());
6708         assertNull(calibrator.getEstimatedMyz());
6709         assertNull(calibrator.getEstimatedMzx());
6710         assertNull(calibrator.getEstimatedMzy());
6711         assertNull(calibrator.getEstimatedCovariance());
6712         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6713         assertNull(calibrator.getGroundTruthGravityNorm());
6714         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6715         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6716 
6717         // Force IllegalArgumentException
6718         calibrator = null;
6719         try {
6720             calibrator = new KnownGravityNormAccelerometerCalibrator(
6721                     measurements, true, new double[1]);
6722             fail("IllegalArgumentException expected but not thrown");
6723         } catch (final IllegalArgumentException ignore) {
6724         }
6725         assertNull(calibrator);
6726     }
6727 
6728     @Test
6729     public void testConstructor62() throws WrongSizeException {
6730         final Collection<StandardDeviationBodyKinematics> measurements =
6731                 Collections.emptyList();
6732 
6733         final Matrix ba = generateBa();
6734         final double[] bias = ba.getBuffer();
6735         final double biasX = ba.getElementAtIndex(0);
6736         final double biasY = ba.getElementAtIndex(1);
6737         final double biasZ = ba.getElementAtIndex(2);
6738 
6739         KnownGravityNormAccelerometerCalibrator calibrator =
6740                 new KnownGravityNormAccelerometerCalibrator(measurements,
6741                         true, bias, this);
6742 
6743         // check default values
6744         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6745         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6746         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6747         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6748         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6749         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6750         final Acceleration bx2 = new Acceleration(0.0,
6751                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6752         calibrator.getInitialBiasXAsAcceleration(bx2);
6753         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6754         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6755         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6756         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6757         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6758         final Acceleration by2 = new Acceleration(0.0,
6759                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6760         calibrator.getInitialBiasYAsAcceleration(by2);
6761         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6762         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6763         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6764         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6765         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6766         final Acceleration bz2 = new Acceleration(0.0,
6767                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6768         calibrator.getInitialBiasZAsAcceleration(bz2);
6769         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6770         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6771         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6772         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6773         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6774         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6775         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6776         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6777         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6778         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6779         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6780         final double[] bias1 = calibrator.getInitialBias();
6781         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6782         final double[] bias2 = new double[3];
6783         calibrator.getInitialBias(bias2);
6784         assertArrayEquals(bias1, bias2, 0.0);
6785         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6786         assertEquals(b1, ba);
6787         final Matrix b2 = new Matrix(3, 1);
6788         calibrator.getInitialBiasAsMatrix(b2);
6789         assertEquals(b1, b2);
6790         final Matrix ma1 = calibrator.getInitialMa();
6791         assertEquals(ma1, new Matrix(3, 3));
6792         final Matrix ma2 = new Matrix(3, 3);
6793         calibrator.getInitialMa(ma2);
6794         assertEquals(ma1, ma2);
6795         assertSame(calibrator.getMeasurements(), measurements);
6796         assertTrue(calibrator.isCommonAxisUsed());
6797         assertSame(calibrator.getListener(), this);
6798         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6799         assertFalse(calibrator.isReady());
6800         assertFalse(calibrator.isRunning());
6801         assertNull(calibrator.getEstimatedBiases());
6802         assertFalse(calibrator.getEstimatedBiases(null));
6803         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6804         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6805         assertNull(calibrator.getEstimatedBiasFx());
6806         assertNull(calibrator.getEstimatedBiasFy());
6807         assertNull(calibrator.getEstimatedBiasFz());
6808         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6809         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6810         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6811         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6812         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6813         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6814         assertNull(calibrator.getEstimatedMa());
6815         assertNull(calibrator.getEstimatedSx());
6816         assertNull(calibrator.getEstimatedSy());
6817         assertNull(calibrator.getEstimatedSz());
6818         assertNull(calibrator.getEstimatedMxy());
6819         assertNull(calibrator.getEstimatedMxz());
6820         assertNull(calibrator.getEstimatedMyx());
6821         assertNull(calibrator.getEstimatedMyz());
6822         assertNull(calibrator.getEstimatedMzx());
6823         assertNull(calibrator.getEstimatedMzy());
6824         assertNull(calibrator.getEstimatedCovariance());
6825         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6826         assertNull(calibrator.getGroundTruthGravityNorm());
6827         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6828         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6829 
6830         // Force IllegalArgumentException
6831         calibrator = null;
6832         try {
6833             calibrator = new KnownGravityNormAccelerometerCalibrator(
6834                     measurements, true, new double[1],
6835                     this);
6836             fail("IllegalArgumentException expected but not thrown");
6837         } catch (final IllegalArgumentException ignore) {
6838         }
6839         assertNull(calibrator);
6840     }
6841 
6842     @Test
6843     public void testConstructor63() throws WrongSizeException {
6844         final Matrix ba = generateBa();
6845         final double biasX = ba.getElementAtIndex(0);
6846         final double biasY = ba.getElementAtIndex(1);
6847         final double biasZ = ba.getElementAtIndex(2);
6848 
6849         KnownGravityNormAccelerometerCalibrator calibrator =
6850                 new KnownGravityNormAccelerometerCalibrator(ba);
6851 
6852         // check default values
6853         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6854         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6855         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6856         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6857         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6858         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6859         final Acceleration bx2 = new Acceleration(0.0,
6860                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6861         calibrator.getInitialBiasXAsAcceleration(bx2);
6862         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6863         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6864         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6865         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6866         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6867         final Acceleration by2 = new Acceleration(0.0,
6868                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6869         calibrator.getInitialBiasYAsAcceleration(by2);
6870         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6871         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6872         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6873         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6874         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6875         final Acceleration bz2 = new Acceleration(0.0,
6876                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6877         calibrator.getInitialBiasZAsAcceleration(bz2);
6878         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6879         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6880         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6881         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6882         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6883         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6884         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6885         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6886         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6887         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6888         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6889         final double[] bias1 = calibrator.getInitialBias();
6890         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6891         final double[] bias2 = new double[3];
6892         calibrator.getInitialBias(bias2);
6893         assertArrayEquals(bias1, bias2, 0.0);
6894         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6895         assertEquals(b1, ba);
6896         final Matrix b2 = new Matrix(3, 1);
6897         calibrator.getInitialBiasAsMatrix(b2);
6898         assertEquals(b1, b2);
6899         final Matrix ma1 = calibrator.getInitialMa();
6900         assertEquals(ma1, new Matrix(3, 3));
6901         final Matrix ma2 = new Matrix(3, 3);
6902         calibrator.getInitialMa(ma2);
6903         assertEquals(ma1, ma2);
6904         assertNull(calibrator.getMeasurements());
6905         assertFalse(calibrator.isCommonAxisUsed());
6906         assertNull(calibrator.getListener());
6907         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6908         assertFalse(calibrator.isReady());
6909         assertFalse(calibrator.isRunning());
6910         assertNull(calibrator.getEstimatedBiases());
6911         assertFalse(calibrator.getEstimatedBiases(null));
6912         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6913         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6914         assertNull(calibrator.getEstimatedBiasFx());
6915         assertNull(calibrator.getEstimatedBiasFy());
6916         assertNull(calibrator.getEstimatedBiasFz());
6917         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6918         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6919         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6920         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6921         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6922         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6923         assertNull(calibrator.getEstimatedMa());
6924         assertNull(calibrator.getEstimatedSx());
6925         assertNull(calibrator.getEstimatedSy());
6926         assertNull(calibrator.getEstimatedSz());
6927         assertNull(calibrator.getEstimatedMxy());
6928         assertNull(calibrator.getEstimatedMxz());
6929         assertNull(calibrator.getEstimatedMyx());
6930         assertNull(calibrator.getEstimatedMyz());
6931         assertNull(calibrator.getEstimatedMzx());
6932         assertNull(calibrator.getEstimatedMzy());
6933         assertNull(calibrator.getEstimatedCovariance());
6934         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6935         assertNull(calibrator.getGroundTruthGravityNorm());
6936         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6937         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6938 
6939         // Force IllegalArgumentException
6940         calibrator = null;
6941         try {
6942             calibrator = new KnownGravityNormAccelerometerCalibrator(
6943                     new Matrix(1, 1));
6944             fail("IllegalArgumentException expected but not thrown");
6945         } catch (final IllegalArgumentException ignore) {
6946         }
6947         try {
6948             calibrator = new KnownGravityNormAccelerometerCalibrator(
6949                     new Matrix(1, 3));
6950             fail("IllegalArgumentException expected but not thrown");
6951         } catch (final IllegalArgumentException ignore) {
6952         }
6953         assertNull(calibrator);
6954     }
6955 
6956     @Test
6957     public void testConstructor64() throws WrongSizeException {
6958         final Matrix ba = generateBa();
6959         final double biasX = ba.getElementAtIndex(0);
6960         final double biasY = ba.getElementAtIndex(1);
6961         final double biasZ = ba.getElementAtIndex(2);
6962 
6963         KnownGravityNormAccelerometerCalibrator calibrator =
6964                 new KnownGravityNormAccelerometerCalibrator(
6965                         ba, this);
6966 
6967         // check default values
6968         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6969         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6970         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6971         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6972         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6973         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6974         final Acceleration bx2 = new Acceleration(0.0,
6975                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6976         calibrator.getInitialBiasXAsAcceleration(bx2);
6977         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6978         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6979         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6980         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6981         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6982         final Acceleration by2 = new Acceleration(0.0,
6983                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6984         calibrator.getInitialBiasYAsAcceleration(by2);
6985         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6986         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6987         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6988         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6989         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6990         final Acceleration bz2 = new Acceleration(0.0,
6991                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6992         calibrator.getInitialBiasZAsAcceleration(bz2);
6993         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6994         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6995         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6996         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6997         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6998         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6999         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7000         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7001         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7002         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7003         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7004         final double[] bias1 = calibrator.getInitialBias();
7005         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7006         final double[] bias2 = new double[3];
7007         calibrator.getInitialBias(bias2);
7008         assertArrayEquals(bias1, bias2, 0.0);
7009         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7010         assertEquals(b1, ba);
7011         final Matrix b2 = new Matrix(3, 1);
7012         calibrator.getInitialBiasAsMatrix(b2);
7013         assertEquals(b1, b2);
7014         final Matrix ma1 = calibrator.getInitialMa();
7015         assertEquals(ma1, new Matrix(3, 3));
7016         final Matrix ma2 = new Matrix(3, 3);
7017         calibrator.getInitialMa(ma2);
7018         assertEquals(ma1, ma2);
7019         assertNull(calibrator.getMeasurements());
7020         assertFalse(calibrator.isCommonAxisUsed());
7021         assertSame(calibrator.getListener(), this);
7022         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7023         assertFalse(calibrator.isReady());
7024         assertFalse(calibrator.isRunning());
7025         assertNull(calibrator.getEstimatedBiases());
7026         assertFalse(calibrator.getEstimatedBiases(null));
7027         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7028         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7029         assertNull(calibrator.getEstimatedBiasFx());
7030         assertNull(calibrator.getEstimatedBiasFy());
7031         assertNull(calibrator.getEstimatedBiasFz());
7032         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7033         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7034         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7035         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7036         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7037         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7038         assertNull(calibrator.getEstimatedMa());
7039         assertNull(calibrator.getEstimatedSx());
7040         assertNull(calibrator.getEstimatedSy());
7041         assertNull(calibrator.getEstimatedSz());
7042         assertNull(calibrator.getEstimatedMxy());
7043         assertNull(calibrator.getEstimatedMxz());
7044         assertNull(calibrator.getEstimatedMyx());
7045         assertNull(calibrator.getEstimatedMyz());
7046         assertNull(calibrator.getEstimatedMzx());
7047         assertNull(calibrator.getEstimatedMzy());
7048         assertNull(calibrator.getEstimatedCovariance());
7049         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7050         assertNull(calibrator.getGroundTruthGravityNorm());
7051         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7052         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7053 
7054         // Force IllegalArgumentException
7055         calibrator = null;
7056         try {
7057             calibrator = new KnownGravityNormAccelerometerCalibrator(
7058                     new Matrix(1, 1));
7059             fail("IllegalArgumentException expected but not thrown");
7060         } catch (final IllegalArgumentException ignore) {
7061         }
7062         try {
7063             calibrator = new KnownGravityNormAccelerometerCalibrator(
7064                     new Matrix(1, 3));
7065             fail("IllegalArgumentException expected but not thrown");
7066         } catch (final IllegalArgumentException ignore) {
7067         }
7068         assertNull(calibrator);
7069     }
7070 
7071     @Test
7072     public void testConstructor65() throws WrongSizeException {
7073         final Collection<StandardDeviationBodyKinematics> measurements =
7074                 Collections.emptyList();
7075 
7076         final Matrix ba = generateBa();
7077         final double biasX = ba.getElementAtIndex(0);
7078         final double biasY = ba.getElementAtIndex(1);
7079         final double biasZ = ba.getElementAtIndex(2);
7080 
7081         KnownGravityNormAccelerometerCalibrator calibrator =
7082                 new KnownGravityNormAccelerometerCalibrator(
7083                         measurements, ba);
7084 
7085         // check default values
7086         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7087         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7088         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7089         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7090         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7091         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7092         final Acceleration bx2 = new Acceleration(0.0,
7093                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7094         calibrator.getInitialBiasXAsAcceleration(bx2);
7095         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7096         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7097         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7098         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7099         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7100         final Acceleration by2 = new Acceleration(0.0,
7101                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7102         calibrator.getInitialBiasYAsAcceleration(by2);
7103         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7104         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7105         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7106         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7107         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7108         final Acceleration bz2 = new Acceleration(0.0,
7109                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7110         calibrator.getInitialBiasZAsAcceleration(bz2);
7111         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7112         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7113         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7114         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7115         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7116         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7117         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7118         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7119         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7120         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7121         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7122         final double[] bias1 = calibrator.getInitialBias();
7123         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7124         final double[] bias2 = new double[3];
7125         calibrator.getInitialBias(bias2);
7126         assertArrayEquals(bias1, bias2, 0.0);
7127         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7128         assertEquals(b1, ba);
7129         final Matrix b2 = new Matrix(3, 1);
7130         calibrator.getInitialBiasAsMatrix(b2);
7131         assertEquals(b1, b2);
7132         final Matrix ma1 = calibrator.getInitialMa();
7133         assertEquals(ma1, new Matrix(3, 3));
7134         final Matrix ma2 = new Matrix(3, 3);
7135         calibrator.getInitialMa(ma2);
7136         assertEquals(ma1, ma2);
7137         assertSame(calibrator.getMeasurements(), measurements);
7138         assertFalse(calibrator.isCommonAxisUsed());
7139         assertNull(calibrator.getListener());
7140         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7141         assertFalse(calibrator.isReady());
7142         assertFalse(calibrator.isRunning());
7143         assertNull(calibrator.getEstimatedBiases());
7144         assertFalse(calibrator.getEstimatedBiases(null));
7145         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7146         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7147         assertNull(calibrator.getEstimatedBiasFx());
7148         assertNull(calibrator.getEstimatedBiasFy());
7149         assertNull(calibrator.getEstimatedBiasFz());
7150         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7151         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7152         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7153         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7154         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7155         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7156         assertNull(calibrator.getEstimatedMa());
7157         assertNull(calibrator.getEstimatedSx());
7158         assertNull(calibrator.getEstimatedSy());
7159         assertNull(calibrator.getEstimatedSz());
7160         assertNull(calibrator.getEstimatedMxy());
7161         assertNull(calibrator.getEstimatedMxz());
7162         assertNull(calibrator.getEstimatedMyx());
7163         assertNull(calibrator.getEstimatedMyz());
7164         assertNull(calibrator.getEstimatedMzx());
7165         assertNull(calibrator.getEstimatedMzy());
7166         assertNull(calibrator.getEstimatedCovariance());
7167         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7168         assertNull(calibrator.getGroundTruthGravityNorm());
7169         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7170         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7171 
7172         // Force IllegalArgumentException
7173         calibrator = null;
7174         try {
7175             calibrator = new KnownGravityNormAccelerometerCalibrator(measurements,
7176                     new Matrix(1, 1));
7177             fail("IllegalArgumentException expected but not thrown");
7178         } catch (final IllegalArgumentException ignore) {
7179         }
7180         try {
7181             calibrator = new KnownGravityNormAccelerometerCalibrator(measurements,
7182                     new Matrix(1, 3));
7183             fail("IllegalArgumentException expected but not thrown");
7184         } catch (final IllegalArgumentException ignore) {
7185         }
7186         assertNull(calibrator);
7187     }
7188 
7189     @Test
7190     public void testConstructor66() throws WrongSizeException {
7191         final Collection<StandardDeviationBodyKinematics> measurements =
7192                 Collections.emptyList();
7193 
7194         final Matrix ba = generateBa();
7195         final double biasX = ba.getElementAtIndex(0);
7196         final double biasY = ba.getElementAtIndex(1);
7197         final double biasZ = ba.getElementAtIndex(2);
7198 
7199         KnownGravityNormAccelerometerCalibrator calibrator =
7200                 new KnownGravityNormAccelerometerCalibrator(measurements, ba,
7201                         this);
7202 
7203         // check default values
7204         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7205         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7206         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7207         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7208         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7209         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7210         final Acceleration bx2 = new Acceleration(0.0,
7211                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7212         calibrator.getInitialBiasXAsAcceleration(bx2);
7213         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7214         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7215         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7216         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7217         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7218         final Acceleration by2 = new Acceleration(0.0,
7219                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7220         calibrator.getInitialBiasYAsAcceleration(by2);
7221         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7222         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7223         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7224         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7225         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7226         final Acceleration bz2 = new Acceleration(0.0,
7227                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7228         calibrator.getInitialBiasZAsAcceleration(bz2);
7229         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7230         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7231         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7232         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7233         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7234         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7235         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7236         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7237         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7238         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7239         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7240         final double[] bias1 = calibrator.getInitialBias();
7241         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7242         final double[] bias2 = new double[3];
7243         calibrator.getInitialBias(bias2);
7244         assertArrayEquals(bias1, bias2, 0.0);
7245         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7246         assertEquals(b1, ba);
7247         final Matrix b2 = new Matrix(3, 1);
7248         calibrator.getInitialBiasAsMatrix(b2);
7249         assertEquals(b1, b2);
7250         final Matrix ma1 = calibrator.getInitialMa();
7251         assertEquals(ma1, new Matrix(3, 3));
7252         final Matrix ma2 = new Matrix(3, 3);
7253         calibrator.getInitialMa(ma2);
7254         assertEquals(ma1, ma2);
7255         assertSame(calibrator.getMeasurements(), measurements);
7256         assertFalse(calibrator.isCommonAxisUsed());
7257         assertSame(calibrator.getListener(), this);
7258         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7259         assertFalse(calibrator.isReady());
7260         assertFalse(calibrator.isRunning());
7261         assertNull(calibrator.getEstimatedBiases());
7262         assertFalse(calibrator.getEstimatedBiases(null));
7263         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7264         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7265         assertNull(calibrator.getEstimatedBiasFx());
7266         assertNull(calibrator.getEstimatedBiasFy());
7267         assertNull(calibrator.getEstimatedBiasFz());
7268         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7269         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7270         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7271         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7272         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7273         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7274         assertNull(calibrator.getEstimatedMa());
7275         assertNull(calibrator.getEstimatedSx());
7276         assertNull(calibrator.getEstimatedSy());
7277         assertNull(calibrator.getEstimatedSz());
7278         assertNull(calibrator.getEstimatedMxy());
7279         assertNull(calibrator.getEstimatedMxz());
7280         assertNull(calibrator.getEstimatedMyx());
7281         assertNull(calibrator.getEstimatedMyz());
7282         assertNull(calibrator.getEstimatedMzx());
7283         assertNull(calibrator.getEstimatedMzy());
7284         assertNull(calibrator.getEstimatedCovariance());
7285         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7286         assertNull(calibrator.getGroundTruthGravityNorm());
7287         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7288         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7289 
7290         // Force IllegalArgumentException
7291         calibrator = null;
7292         try {
7293             calibrator = new KnownGravityNormAccelerometerCalibrator(
7294                     measurements, new Matrix(1, 1),
7295                     this);
7296             fail("IllegalArgumentException expected but not thrown");
7297         } catch (final IllegalArgumentException ignore) {
7298         }
7299         try {
7300             calibrator = new KnownGravityNormAccelerometerCalibrator(
7301                     measurements, new Matrix(1, 3),
7302                     this);
7303             fail("IllegalArgumentException expected but not thrown");
7304         } catch (final IllegalArgumentException ignore) {
7305         }
7306         assertNull(calibrator);
7307     }
7308 
7309     @Test
7310     public void testConstructor67() throws WrongSizeException {
7311         final Matrix ba = generateBa();
7312         final double biasX = ba.getElementAtIndex(0);
7313         final double biasY = ba.getElementAtIndex(1);
7314         final double biasZ = ba.getElementAtIndex(2);
7315 
7316         KnownGravityNormAccelerometerCalibrator calibrator =
7317                 new KnownGravityNormAccelerometerCalibrator(
7318                         true, ba);
7319 
7320         // check default values
7321         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7322         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7323         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7324         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7325         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7326         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7327         final Acceleration bx2 = new Acceleration(0.0,
7328                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7329         calibrator.getInitialBiasXAsAcceleration(bx2);
7330         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7331         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7332         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7333         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7334         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7335         final Acceleration by2 = new Acceleration(0.0,
7336                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7337         calibrator.getInitialBiasYAsAcceleration(by2);
7338         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7339         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7340         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7341         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7342         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7343         final Acceleration bz2 = new Acceleration(0.0,
7344                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7345         calibrator.getInitialBiasZAsAcceleration(bz2);
7346         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7347         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7348         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7349         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7350         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7351         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7352         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7353         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7354         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7355         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7356         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7357         final double[] bias1 = calibrator.getInitialBias();
7358         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7359         final double[] bias2 = new double[3];
7360         calibrator.getInitialBias(bias2);
7361         assertArrayEquals(bias1, bias2, 0.0);
7362         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7363         assertEquals(b1, ba);
7364         final Matrix b2 = new Matrix(3, 1);
7365         calibrator.getInitialBiasAsMatrix(b2);
7366         assertEquals(b1, b2);
7367         final Matrix ma1 = calibrator.getInitialMa();
7368         assertEquals(ma1, new Matrix(3, 3));
7369         final Matrix ma2 = new Matrix(3, 3);
7370         calibrator.getInitialMa(ma2);
7371         assertEquals(ma1, ma2);
7372         assertNull(calibrator.getMeasurements());
7373         assertTrue(calibrator.isCommonAxisUsed());
7374         assertNull(calibrator.getListener());
7375         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7376         assertFalse(calibrator.isReady());
7377         assertFalse(calibrator.isRunning());
7378         assertNull(calibrator.getEstimatedBiases());
7379         assertFalse(calibrator.getEstimatedBiases(null));
7380         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7381         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7382         assertNull(calibrator.getEstimatedBiasFx());
7383         assertNull(calibrator.getEstimatedBiasFy());
7384         assertNull(calibrator.getEstimatedBiasFz());
7385         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7386         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7387         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7388         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7389         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7390         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7391         assertNull(calibrator.getEstimatedMa());
7392         assertNull(calibrator.getEstimatedSx());
7393         assertNull(calibrator.getEstimatedSy());
7394         assertNull(calibrator.getEstimatedSz());
7395         assertNull(calibrator.getEstimatedMxy());
7396         assertNull(calibrator.getEstimatedMxz());
7397         assertNull(calibrator.getEstimatedMyx());
7398         assertNull(calibrator.getEstimatedMyz());
7399         assertNull(calibrator.getEstimatedMzx());
7400         assertNull(calibrator.getEstimatedMzy());
7401         assertNull(calibrator.getEstimatedCovariance());
7402         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7403         assertNull(calibrator.getGroundTruthGravityNorm());
7404         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7405         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7406 
7407         // Force IllegalArgumentException
7408         calibrator = null;
7409         try {
7410             calibrator = new KnownGravityNormAccelerometerCalibrator(
7411                     true, new Matrix(1, 1));
7412             fail("IllegalArgumentException expected but not thrown");
7413         } catch (final IllegalArgumentException ignore) {
7414         }
7415         try {
7416             calibrator = new KnownGravityNormAccelerometerCalibrator(
7417                     true, new Matrix(1, 3));
7418             fail("IllegalArgumentException expected but not thrown");
7419         } catch (final IllegalArgumentException ignore) {
7420         }
7421         assertNull(calibrator);
7422     }
7423 
7424     @Test
7425     public void testConstructor68() throws WrongSizeException {
7426         final Matrix ba = generateBa();
7427         final double biasX = ba.getElementAtIndex(0);
7428         final double biasY = ba.getElementAtIndex(1);
7429         final double biasZ = ba.getElementAtIndex(2);
7430 
7431         KnownGravityNormAccelerometerCalibrator calibrator =
7432                 new KnownGravityNormAccelerometerCalibrator(
7433                         true, ba, this);
7434 
7435         // check default values
7436         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7437         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7438         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7439         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7440         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7441         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7442         final Acceleration bx2 = new Acceleration(0.0,
7443                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7444         calibrator.getInitialBiasXAsAcceleration(bx2);
7445         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7446         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7447         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7448         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7449         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7450         final Acceleration by2 = new Acceleration(0.0,
7451                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7452         calibrator.getInitialBiasYAsAcceleration(by2);
7453         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7454         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7455         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7456         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7457         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7458         final Acceleration bz2 = new Acceleration(0.0,
7459                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7460         calibrator.getInitialBiasZAsAcceleration(bz2);
7461         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7462         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7463         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7464         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7465         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7466         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7467         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7468         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7469         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7470         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7471         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7472         final double[] bias1 = calibrator.getInitialBias();
7473         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7474         final double[] bias2 = new double[3];
7475         calibrator.getInitialBias(bias2);
7476         assertArrayEquals(bias1, bias2, 0.0);
7477         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7478         assertEquals(b1, ba);
7479         final Matrix b2 = new Matrix(3, 1);
7480         calibrator.getInitialBiasAsMatrix(b2);
7481         assertEquals(b1, b2);
7482         final Matrix ma1 = calibrator.getInitialMa();
7483         assertEquals(ma1, new Matrix(3, 3));
7484         final Matrix ma2 = new Matrix(3, 3);
7485         calibrator.getInitialMa(ma2);
7486         assertEquals(ma1, ma2);
7487         assertNull(calibrator.getMeasurements());
7488         assertTrue(calibrator.isCommonAxisUsed());
7489         assertSame(calibrator.getListener(), this);
7490         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7491         assertFalse(calibrator.isReady());
7492         assertFalse(calibrator.isRunning());
7493         assertNull(calibrator.getEstimatedBiases());
7494         assertFalse(calibrator.getEstimatedBiases(null));
7495         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7496         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7497         assertNull(calibrator.getEstimatedBiasFx());
7498         assertNull(calibrator.getEstimatedBiasFy());
7499         assertNull(calibrator.getEstimatedBiasFz());
7500         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7501         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7502         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7503         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7504         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7505         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7506         assertNull(calibrator.getEstimatedMa());
7507         assertNull(calibrator.getEstimatedSx());
7508         assertNull(calibrator.getEstimatedSy());
7509         assertNull(calibrator.getEstimatedSz());
7510         assertNull(calibrator.getEstimatedMxy());
7511         assertNull(calibrator.getEstimatedMxz());
7512         assertNull(calibrator.getEstimatedMyx());
7513         assertNull(calibrator.getEstimatedMyz());
7514         assertNull(calibrator.getEstimatedMzx());
7515         assertNull(calibrator.getEstimatedMzy());
7516         assertNull(calibrator.getEstimatedCovariance());
7517         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7518         assertNull(calibrator.getGroundTruthGravityNorm());
7519         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7520         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7521 
7522         // Force IllegalArgumentException
7523         calibrator = null;
7524         try {
7525             calibrator = new KnownGravityNormAccelerometerCalibrator(
7526                     true, new Matrix(1, 1),
7527                     this);
7528             fail("IllegalArgumentException expected but not thrown");
7529         } catch (final IllegalArgumentException ignore) {
7530         }
7531         try {
7532             calibrator = new KnownGravityNormAccelerometerCalibrator(
7533                     true, new Matrix(1, 3),
7534                     this);
7535             fail("IllegalArgumentException expected but not thrown");
7536         } catch (final IllegalArgumentException ignore) {
7537         }
7538         assertNull(calibrator);
7539     }
7540 
7541     @Test
7542     public void testConstructor69() throws WrongSizeException {
7543         final Collection<StandardDeviationBodyKinematics> measurements =
7544                 Collections.emptyList();
7545 
7546         final Matrix ba = generateBa();
7547         final double biasX = ba.getElementAtIndex(0);
7548         final double biasY = ba.getElementAtIndex(1);
7549         final double biasZ = ba.getElementAtIndex(2);
7550 
7551         KnownGravityNormAccelerometerCalibrator calibrator =
7552                 new KnownGravityNormAccelerometerCalibrator(measurements,
7553                         true, ba);
7554 
7555         // check default values
7556         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7557         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7558         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7559         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7560         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7561         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7562         final Acceleration bx2 = new Acceleration(0.0,
7563                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7564         calibrator.getInitialBiasXAsAcceleration(bx2);
7565         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7566         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7567         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7568         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7569         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7570         final Acceleration by2 = new Acceleration(0.0,
7571                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7572         calibrator.getInitialBiasYAsAcceleration(by2);
7573         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7574         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7575         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7576         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7577         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7578         final Acceleration bz2 = new Acceleration(0.0,
7579                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7580         calibrator.getInitialBiasZAsAcceleration(bz2);
7581         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7582         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7583         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7584         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7585         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7586         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7587         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7588         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7589         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7590         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7591         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7592         final double[] bias1 = calibrator.getInitialBias();
7593         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7594         final double[] bias2 = new double[3];
7595         calibrator.getInitialBias(bias2);
7596         assertArrayEquals(bias1, bias2, 0.0);
7597         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7598         assertEquals(b1, ba);
7599         final Matrix b2 = new Matrix(3, 1);
7600         calibrator.getInitialBiasAsMatrix(b2);
7601         assertEquals(b1, b2);
7602         final Matrix ma1 = calibrator.getInitialMa();
7603         assertEquals(ma1, new Matrix(3, 3));
7604         final Matrix ma2 = new Matrix(3, 3);
7605         calibrator.getInitialMa(ma2);
7606         assertEquals(ma1, ma2);
7607         assertSame(calibrator.getMeasurements(), measurements);
7608         assertTrue(calibrator.isCommonAxisUsed());
7609         assertNull(calibrator.getListener());
7610         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7611         assertFalse(calibrator.isReady());
7612         assertFalse(calibrator.isRunning());
7613         assertNull(calibrator.getEstimatedBiases());
7614         assertFalse(calibrator.getEstimatedBiases(null));
7615         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7616         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7617         assertNull(calibrator.getEstimatedBiasFx());
7618         assertNull(calibrator.getEstimatedBiasFy());
7619         assertNull(calibrator.getEstimatedBiasFz());
7620         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7621         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7622         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7623         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7624         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7625         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7626         assertNull(calibrator.getEstimatedMa());
7627         assertNull(calibrator.getEstimatedSx());
7628         assertNull(calibrator.getEstimatedSy());
7629         assertNull(calibrator.getEstimatedSz());
7630         assertNull(calibrator.getEstimatedMxy());
7631         assertNull(calibrator.getEstimatedMxz());
7632         assertNull(calibrator.getEstimatedMyx());
7633         assertNull(calibrator.getEstimatedMyz());
7634         assertNull(calibrator.getEstimatedMzx());
7635         assertNull(calibrator.getEstimatedMzy());
7636         assertNull(calibrator.getEstimatedCovariance());
7637         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7638         assertNull(calibrator.getGroundTruthGravityNorm());
7639         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7640         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7641 
7642         // Force IllegalArgumentException
7643         calibrator = null;
7644         try {
7645             calibrator = new KnownGravityNormAccelerometerCalibrator(
7646                     measurements, true,
7647                     new Matrix(1, 1));
7648             fail("IllegalArgumentException expected but not thrown");
7649         } catch (final IllegalArgumentException ignore) {
7650         }
7651         try {
7652             calibrator = new KnownGravityNormAccelerometerCalibrator(
7653                     measurements, true,
7654                     new Matrix(1, 3));
7655             fail("IllegalArgumentException expected but not thrown");
7656         } catch (final IllegalArgumentException ignore) {
7657         }
7658         assertNull(calibrator);
7659     }
7660 
7661     @Test
7662     public void testConstructor70() throws WrongSizeException {
7663         final Collection<StandardDeviationBodyKinematics> measurements =
7664                 Collections.emptyList();
7665 
7666         final Matrix ba = generateBa();
7667         final double biasX = ba.getElementAtIndex(0);
7668         final double biasY = ba.getElementAtIndex(1);
7669         final double biasZ = ba.getElementAtIndex(2);
7670 
7671         KnownGravityNormAccelerometerCalibrator calibrator =
7672                 new KnownGravityNormAccelerometerCalibrator(measurements,
7673                         true, ba, this);
7674 
7675         // check default values
7676         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7677         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7678         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7679         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7680         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7681         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7682         final Acceleration bx2 = new Acceleration(0.0,
7683                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7684         calibrator.getInitialBiasXAsAcceleration(bx2);
7685         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7686         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7687         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7688         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7689         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7690         final Acceleration by2 = new Acceleration(0.0,
7691                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7692         calibrator.getInitialBiasYAsAcceleration(by2);
7693         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7694         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7695         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7696         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7697         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7698         final Acceleration bz2 = new Acceleration(0.0,
7699                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7700         calibrator.getInitialBiasZAsAcceleration(bz2);
7701         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7702         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7703         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7704         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7705         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7706         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7707         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7708         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7709         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7710         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7711         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7712         final double[] bias1 = calibrator.getInitialBias();
7713         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7714         final double[] bias2 = new double[3];
7715         calibrator.getInitialBias(bias2);
7716         assertArrayEquals(bias1, bias2, 0.0);
7717         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7718         assertEquals(b1, ba);
7719         final Matrix b2 = new Matrix(3, 1);
7720         calibrator.getInitialBiasAsMatrix(b2);
7721         assertEquals(b1, b2);
7722         final Matrix ma1 = calibrator.getInitialMa();
7723         assertEquals(ma1, new Matrix(3, 3));
7724         final Matrix ma2 = new Matrix(3, 3);
7725         calibrator.getInitialMa(ma2);
7726         assertEquals(ma1, ma2);
7727         assertSame(calibrator.getMeasurements(), measurements);
7728         assertTrue(calibrator.isCommonAxisUsed());
7729         assertSame(calibrator.getListener(), this);
7730         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7731         assertFalse(calibrator.isReady());
7732         assertFalse(calibrator.isRunning());
7733         assertNull(calibrator.getEstimatedBiases());
7734         assertFalse(calibrator.getEstimatedBiases(null));
7735         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7736         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7737         assertNull(calibrator.getEstimatedBiasFx());
7738         assertNull(calibrator.getEstimatedBiasFy());
7739         assertNull(calibrator.getEstimatedBiasFz());
7740         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7741         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7742         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7743         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7744         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7745         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7746         assertNull(calibrator.getEstimatedMa());
7747         assertNull(calibrator.getEstimatedSx());
7748         assertNull(calibrator.getEstimatedSy());
7749         assertNull(calibrator.getEstimatedSz());
7750         assertNull(calibrator.getEstimatedMxy());
7751         assertNull(calibrator.getEstimatedMxz());
7752         assertNull(calibrator.getEstimatedMyx());
7753         assertNull(calibrator.getEstimatedMyz());
7754         assertNull(calibrator.getEstimatedMzx());
7755         assertNull(calibrator.getEstimatedMzy());
7756         assertNull(calibrator.getEstimatedCovariance());
7757         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7758         assertNull(calibrator.getGroundTruthGravityNorm());
7759         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7760         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7761 
7762         // Force IllegalArgumentException
7763         calibrator = null;
7764         try {
7765             calibrator = new KnownGravityNormAccelerometerCalibrator(
7766                     measurements, true,
7767                     new Matrix(1, 1), this);
7768             fail("IllegalArgumentException expected but not thrown");
7769         } catch (final IllegalArgumentException ignore) {
7770         }
7771         try {
7772             calibrator = new KnownGravityNormAccelerometerCalibrator(
7773                     measurements, true,
7774                     new Matrix(1, 3), this);
7775             fail("IllegalArgumentException expected but not thrown");
7776         } catch (final IllegalArgumentException ignore) {
7777         }
7778         assertNull(calibrator);
7779     }
7780 
7781     @Test
7782     public void testConstructor71() throws WrongSizeException {
7783         final Matrix ba = generateBa();
7784         final double biasX = ba.getElementAtIndex(0);
7785         final double biasY = ba.getElementAtIndex(1);
7786         final double biasZ = ba.getElementAtIndex(2);
7787 
7788         final Matrix ma = generateMaCommonAxis();
7789         final double sx = ma.getElementAt(0, 0);
7790         final double sy = ma.getElementAt(1, 1);
7791         final double sz = ma.getElementAt(2, 2);
7792         final double mxy = ma.getElementAt(0, 1);
7793         final double mxz = ma.getElementAt(0, 2);
7794         final double myx = ma.getElementAt(1, 0);
7795         final double myz = ma.getElementAt(1, 2);
7796         final double mzx = ma.getElementAt(2, 0);
7797         final double mzy = ma.getElementAt(2, 1);
7798 
7799         KnownGravityNormAccelerometerCalibrator calibrator =
7800                 new KnownGravityNormAccelerometerCalibrator(ba, ma);
7801 
7802         // check default values
7803         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7804         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7805         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7806         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7807         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7808         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7809         final Acceleration bx2 = new Acceleration(0.0,
7810                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7811         calibrator.getInitialBiasXAsAcceleration(bx2);
7812         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7813         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7814         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7815         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7816         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7817         final Acceleration by2 = new Acceleration(0.0,
7818                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7819         calibrator.getInitialBiasYAsAcceleration(by2);
7820         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7821         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7822         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7823         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7824         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7825         final Acceleration bz2 = new Acceleration(0.0,
7826                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7827         calibrator.getInitialBiasZAsAcceleration(bz2);
7828         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7829         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7830         assertEquals(calibrator.getInitialSx(), sx, 0.0);
7831         assertEquals(calibrator.getInitialSy(), sy, 0.0);
7832         assertEquals(calibrator.getInitialSz(), sz, 0.0);
7833         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7834         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7835         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7836         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7837         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7838         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7839         final double[] bias1 = calibrator.getInitialBias();
7840         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7841         final double[] bias2 = new double[3];
7842         calibrator.getInitialBias(bias2);
7843         assertArrayEquals(bias1, bias2, 0.0);
7844         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7845         assertEquals(b1, ba);
7846         final Matrix b2 = new Matrix(3, 1);
7847         calibrator.getInitialBiasAsMatrix(b2);
7848         assertEquals(b1, b2);
7849         final Matrix ma1 = new Matrix(3, 3);
7850         ma1.setSubmatrix(0, 0,
7851                 2, 2,
7852                 new double[]{sx, myx, mzx,
7853                         mxy, sy, mzy,
7854                         mxz, myz, sz});
7855         assertEquals(calibrator.getInitialMa(), ma1);
7856         final Matrix ma2 = new Matrix(3, 3);
7857         calibrator.getInitialMa(ma2);
7858         assertEquals(ma1, ma2);
7859         assertNull(calibrator.getMeasurements());
7860         assertFalse(calibrator.isCommonAxisUsed());
7861         assertNull(calibrator.getListener());
7862         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7863         assertFalse(calibrator.isReady());
7864         assertFalse(calibrator.isRunning());
7865         assertNull(calibrator.getEstimatedBiases());
7866         assertFalse(calibrator.getEstimatedBiases(null));
7867         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7868         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7869         assertNull(calibrator.getEstimatedBiasFx());
7870         assertNull(calibrator.getEstimatedBiasFy());
7871         assertNull(calibrator.getEstimatedBiasFz());
7872         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7873         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7874         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7875         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7876         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7877         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7878         assertNull(calibrator.getEstimatedMa());
7879         assertNull(calibrator.getEstimatedSx());
7880         assertNull(calibrator.getEstimatedSy());
7881         assertNull(calibrator.getEstimatedSz());
7882         assertNull(calibrator.getEstimatedMxy());
7883         assertNull(calibrator.getEstimatedMxz());
7884         assertNull(calibrator.getEstimatedMyx());
7885         assertNull(calibrator.getEstimatedMyz());
7886         assertNull(calibrator.getEstimatedMzx());
7887         assertNull(calibrator.getEstimatedMzy());
7888         assertNull(calibrator.getEstimatedCovariance());
7889         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7890         assertNull(calibrator.getGroundTruthGravityNorm());
7891         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7892         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7893 
7894         // Force IllegalArgumentException
7895         calibrator = null;
7896         try {
7897             calibrator = new KnownGravityNormAccelerometerCalibrator(
7898                     new Matrix(1, 1), ma);
7899             fail("IllegalArgumentException expected but not thrown");
7900         } catch (final IllegalArgumentException ignore) {
7901         }
7902         try {
7903             calibrator = new KnownGravityNormAccelerometerCalibrator(
7904                     new Matrix(1, 3), ma);
7905             fail("IllegalArgumentException expected but not thrown");
7906         } catch (final IllegalArgumentException ignore) {
7907         }
7908         try {
7909             calibrator = new KnownGravityNormAccelerometerCalibrator(ba,
7910                     new Matrix(1, 3));
7911             fail("IllegalArgumentException expected but not thrown");
7912         } catch (final IllegalArgumentException ignore) {
7913         }
7914         try {
7915             calibrator = new KnownGravityNormAccelerometerCalibrator(ba,
7916                     new Matrix(3, 1));
7917             fail("IllegalArgumentException expected but not thrown");
7918         } catch (final IllegalArgumentException ignore) {
7919         }
7920         assertNull(calibrator);
7921     }
7922 
7923     @Test
7924     public void testConstructor72() throws WrongSizeException {
7925         final Matrix ba = generateBa();
7926         final double biasX = ba.getElementAtIndex(0);
7927         final double biasY = ba.getElementAtIndex(1);
7928         final double biasZ = ba.getElementAtIndex(2);
7929 
7930         final Matrix ma = generateMaCommonAxis();
7931         final double sx = ma.getElementAt(0, 0);
7932         final double sy = ma.getElementAt(1, 1);
7933         final double sz = ma.getElementAt(2, 2);
7934         final double mxy = ma.getElementAt(0, 1);
7935         final double mxz = ma.getElementAt(0, 2);
7936         final double myx = ma.getElementAt(1, 0);
7937         final double myz = ma.getElementAt(1, 2);
7938         final double mzx = ma.getElementAt(2, 0);
7939         final double mzy = ma.getElementAt(2, 1);
7940 
7941         KnownGravityNormAccelerometerCalibrator calibrator =
7942                 new KnownGravityNormAccelerometerCalibrator(
7943                         ba, ma, this);
7944 
7945         // check default values
7946         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7947         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7948         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7949         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7950         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7951         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7952         final Acceleration bx2 = new Acceleration(0.0,
7953                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7954         calibrator.getInitialBiasXAsAcceleration(bx2);
7955         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7956         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7957         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7958         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7959         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7960         final Acceleration by2 = new Acceleration(0.0,
7961                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7962         calibrator.getInitialBiasYAsAcceleration(by2);
7963         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7964         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7965         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7966         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7967         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7968         final Acceleration bz2 = new Acceleration(0.0,
7969                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7970         calibrator.getInitialBiasZAsAcceleration(bz2);
7971         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7972         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7973         assertEquals(calibrator.getInitialSx(), sx, 0.0);
7974         assertEquals(calibrator.getInitialSy(), sy, 0.0);
7975         assertEquals(calibrator.getInitialSz(), sz, 0.0);
7976         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
7977         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
7978         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
7979         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
7980         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
7981         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
7982         final double[] bias1 = calibrator.getInitialBias();
7983         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7984         final double[] bias2 = new double[3];
7985         calibrator.getInitialBias(bias2);
7986         assertArrayEquals(bias1, bias2, 0.0);
7987         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7988         assertEquals(b1, ba);
7989         final Matrix b2 = new Matrix(3, 1);
7990         calibrator.getInitialBiasAsMatrix(b2);
7991         assertEquals(b1, b2);
7992         final Matrix ma1 = new Matrix(3, 3);
7993         ma1.setSubmatrix(0, 0,
7994                 2, 2,
7995                 new double[]{sx, myx, mzx,
7996                         mxy, sy, mzy,
7997                         mxz, myz, sz});
7998         assertEquals(calibrator.getInitialMa(), ma1);
7999         final Matrix ma2 = new Matrix(3, 3);
8000         calibrator.getInitialMa(ma2);
8001         assertEquals(ma1, ma2);
8002         assertNull(calibrator.getMeasurements());
8003         assertFalse(calibrator.isCommonAxisUsed());
8004         assertSame(calibrator.getListener(), this);
8005         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8006         assertFalse(calibrator.isReady());
8007         assertFalse(calibrator.isRunning());
8008         assertNull(calibrator.getEstimatedBiases());
8009         assertFalse(calibrator.getEstimatedBiases(null));
8010         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8011         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8012         assertNull(calibrator.getEstimatedBiasFx());
8013         assertNull(calibrator.getEstimatedBiasFy());
8014         assertNull(calibrator.getEstimatedBiasFz());
8015         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8016         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8017         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8018         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8019         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8020         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8021         assertNull(calibrator.getEstimatedMa());
8022         assertNull(calibrator.getEstimatedSx());
8023         assertNull(calibrator.getEstimatedSy());
8024         assertNull(calibrator.getEstimatedSz());
8025         assertNull(calibrator.getEstimatedMxy());
8026         assertNull(calibrator.getEstimatedMxz());
8027         assertNull(calibrator.getEstimatedMyx());
8028         assertNull(calibrator.getEstimatedMyz());
8029         assertNull(calibrator.getEstimatedMzx());
8030         assertNull(calibrator.getEstimatedMzy());
8031         assertNull(calibrator.getEstimatedCovariance());
8032         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8033         assertNull(calibrator.getGroundTruthGravityNorm());
8034         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8035         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8036 
8037         // Force IllegalArgumentException
8038         calibrator = null;
8039         try {
8040             calibrator = new KnownGravityNormAccelerometerCalibrator(
8041                     new Matrix(1, 1), ma, this);
8042             fail("IllegalArgumentException expected but not thrown");
8043         } catch (final IllegalArgumentException ignore) {
8044         }
8045         try {
8046             calibrator = new KnownGravityNormAccelerometerCalibrator(
8047                     new Matrix(1, 3), ma, this);
8048             fail("IllegalArgumentException expected but not thrown");
8049         } catch (final IllegalArgumentException ignore) {
8050         }
8051         try {
8052             calibrator = new KnownGravityNormAccelerometerCalibrator(ba,
8053                     new Matrix(1, 3), this);
8054             fail("IllegalArgumentException expected but not thrown");
8055         } catch (final IllegalArgumentException ignore) {
8056         }
8057         try {
8058             calibrator = new KnownGravityNormAccelerometerCalibrator(ba,
8059                     new Matrix(3, 1), this);
8060             fail("IllegalArgumentException expected but not thrown");
8061         } catch (final IllegalArgumentException ignore) {
8062         }
8063         assertNull(calibrator);
8064     }
8065 
8066     @Test
8067     public void testConstructor73() throws WrongSizeException {
8068         final Collection<StandardDeviationBodyKinematics> measurements =
8069                 Collections.emptyList();
8070 
8071         final Matrix ba = generateBa();
8072         final double biasX = ba.getElementAtIndex(0);
8073         final double biasY = ba.getElementAtIndex(1);
8074         final double biasZ = ba.getElementAtIndex(2);
8075 
8076         final Matrix ma = generateMaCommonAxis();
8077         final double sx = ma.getElementAt(0, 0);
8078         final double sy = ma.getElementAt(1, 1);
8079         final double sz = ma.getElementAt(2, 2);
8080         final double mxy = ma.getElementAt(0, 1);
8081         final double mxz = ma.getElementAt(0, 2);
8082         final double myx = ma.getElementAt(1, 0);
8083         final double myz = ma.getElementAt(1, 2);
8084         final double mzx = ma.getElementAt(2, 0);
8085         final double mzy = ma.getElementAt(2, 1);
8086 
8087         KnownGravityNormAccelerometerCalibrator calibrator =
8088                 new KnownGravityNormAccelerometerCalibrator(
8089                         measurements, ba, ma);
8090 
8091         // check default values
8092         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8093         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8094         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8095         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8096         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8097         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8098         final Acceleration bx2 = new Acceleration(0.0,
8099                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8100         calibrator.getInitialBiasXAsAcceleration(bx2);
8101         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8102         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8103         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8104         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8105         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8106         final Acceleration by2 = new Acceleration(0.0,
8107                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8108         calibrator.getInitialBiasYAsAcceleration(by2);
8109         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8110         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8111         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8112         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8113         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8114         final Acceleration bz2 = new Acceleration(0.0,
8115                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8116         calibrator.getInitialBiasZAsAcceleration(bz2);
8117         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8118         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8119         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8120         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8121         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8122         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8123         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8124         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8125         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8126         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8127         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8128         final double[] bias1 = calibrator.getInitialBias();
8129         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8130         final double[] bias2 = new double[3];
8131         calibrator.getInitialBias(bias2);
8132         assertArrayEquals(bias1, bias2, 0.0);
8133         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8134         assertEquals(b1, ba);
8135         final Matrix b2 = new Matrix(3, 1);
8136         calibrator.getInitialBiasAsMatrix(b2);
8137         assertEquals(b1, b2);
8138         final Matrix ma1 = new Matrix(3, 3);
8139         ma1.setSubmatrix(0, 0,
8140                 2, 2,
8141                 new double[]{sx, myx, mzx,
8142                         mxy, sy, mzy,
8143                         mxz, myz, sz});
8144         assertEquals(calibrator.getInitialMa(), ma1);
8145         final Matrix ma2 = new Matrix(3, 3);
8146         calibrator.getInitialMa(ma2);
8147         assertEquals(ma1, ma2);
8148         assertSame(calibrator.getMeasurements(), measurements);
8149         assertFalse(calibrator.isCommonAxisUsed());
8150         assertNull(calibrator.getListener());
8151         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8152         assertFalse(calibrator.isReady());
8153         assertFalse(calibrator.isRunning());
8154         assertNull(calibrator.getEstimatedBiases());
8155         assertFalse(calibrator.getEstimatedBiases(null));
8156         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8157         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8158         assertNull(calibrator.getEstimatedBiasFx());
8159         assertNull(calibrator.getEstimatedBiasFy());
8160         assertNull(calibrator.getEstimatedBiasFz());
8161         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8162         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8163         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8164         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8165         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8166         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8167         assertNull(calibrator.getEstimatedMa());
8168         assertNull(calibrator.getEstimatedSx());
8169         assertNull(calibrator.getEstimatedSy());
8170         assertNull(calibrator.getEstimatedSz());
8171         assertNull(calibrator.getEstimatedMxy());
8172         assertNull(calibrator.getEstimatedMxz());
8173         assertNull(calibrator.getEstimatedMyx());
8174         assertNull(calibrator.getEstimatedMyz());
8175         assertNull(calibrator.getEstimatedMzx());
8176         assertNull(calibrator.getEstimatedMzy());
8177         assertNull(calibrator.getEstimatedCovariance());
8178         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8179         assertNull(calibrator.getGroundTruthGravityNorm());
8180         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8181         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8182 
8183         // Force IllegalArgumentException
8184         calibrator = null;
8185         try {
8186             calibrator = new KnownGravityNormAccelerometerCalibrator(
8187                     measurements, new Matrix(1, 1), ma);
8188             fail("IllegalArgumentException expected but not thrown");
8189         } catch (final IllegalArgumentException ignore) {
8190         }
8191         try {
8192             calibrator = new KnownGravityNormAccelerometerCalibrator(
8193                     measurements, new Matrix(1, 3), ma);
8194             fail("IllegalArgumentException expected but not thrown");
8195         } catch (final IllegalArgumentException ignore) {
8196         }
8197         try {
8198             calibrator = new KnownGravityNormAccelerometerCalibrator(
8199                     measurements, ba, new Matrix(1, 3));
8200             fail("IllegalArgumentException expected but not thrown");
8201         } catch (final IllegalArgumentException ignore) {
8202         }
8203         try {
8204             calibrator = new KnownGravityNormAccelerometerCalibrator(
8205                     measurements, ba, new Matrix(3, 1));
8206             fail("IllegalArgumentException expected but not thrown");
8207         } catch (final IllegalArgumentException ignore) {
8208         }
8209         assertNull(calibrator);
8210     }
8211 
8212     @Test
8213     public void testConstructor74() throws WrongSizeException {
8214         final Collection<StandardDeviationBodyKinematics> measurements =
8215                 Collections.emptyList();
8216 
8217         final Matrix ba = generateBa();
8218         final double biasX = ba.getElementAtIndex(0);
8219         final double biasY = ba.getElementAtIndex(1);
8220         final double biasZ = ba.getElementAtIndex(2);
8221 
8222         final Matrix ma = generateMaCommonAxis();
8223         final double sx = ma.getElementAt(0, 0);
8224         final double sy = ma.getElementAt(1, 1);
8225         final double sz = ma.getElementAt(2, 2);
8226         final double mxy = ma.getElementAt(0, 1);
8227         final double mxz = ma.getElementAt(0, 2);
8228         final double myx = ma.getElementAt(1, 0);
8229         final double myz = ma.getElementAt(1, 2);
8230         final double mzx = ma.getElementAt(2, 0);
8231         final double mzy = ma.getElementAt(2, 1);
8232 
8233         KnownGravityNormAccelerometerCalibrator calibrator =
8234                 new KnownGravityNormAccelerometerCalibrator(
8235                         measurements, ba, ma, this);
8236 
8237         // check default values
8238         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8239         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8240         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8241         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8242         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8243         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8244         final Acceleration bx2 = new Acceleration(0.0,
8245                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8246         calibrator.getInitialBiasXAsAcceleration(bx2);
8247         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8248         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8249         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8250         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8251         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8252         final Acceleration by2 = new Acceleration(0.0,
8253                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8254         calibrator.getInitialBiasYAsAcceleration(by2);
8255         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8256         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8257         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8258         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8259         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8260         final Acceleration bz2 = new Acceleration(0.0,
8261                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8262         calibrator.getInitialBiasZAsAcceleration(bz2);
8263         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8264         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8265         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8266         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8267         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8268         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8269         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8270         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8271         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8272         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8273         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8274         final double[] bias1 = calibrator.getInitialBias();
8275         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8276         final double[] bias2 = new double[3];
8277         calibrator.getInitialBias(bias2);
8278         assertArrayEquals(bias1, bias2, 0.0);
8279         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8280         assertEquals(b1, ba);
8281         final Matrix b2 = new Matrix(3, 1);
8282         calibrator.getInitialBiasAsMatrix(b2);
8283         assertEquals(b1, b2);
8284         final Matrix ma1 = new Matrix(3, 3);
8285         ma1.setSubmatrix(0, 0,
8286                 2, 2,
8287                 new double[]{sx, myx, mzx,
8288                         mxy, sy, mzy,
8289                         mxz, myz, sz});
8290         assertEquals(calibrator.getInitialMa(), ma1);
8291         final Matrix ma2 = new Matrix(3, 3);
8292         calibrator.getInitialMa(ma2);
8293         assertEquals(ma1, ma2);
8294         assertSame(calibrator.getMeasurements(), measurements);
8295         assertFalse(calibrator.isCommonAxisUsed());
8296         assertSame(calibrator.getListener(), this);
8297         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8298         assertFalse(calibrator.isReady());
8299         assertFalse(calibrator.isRunning());
8300         assertNull(calibrator.getEstimatedBiases());
8301         assertFalse(calibrator.getEstimatedBiases(null));
8302         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8303         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8304         assertNull(calibrator.getEstimatedBiasFx());
8305         assertNull(calibrator.getEstimatedBiasFy());
8306         assertNull(calibrator.getEstimatedBiasFz());
8307         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8308         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8309         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8310         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8311         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8312         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8313         assertNull(calibrator.getEstimatedMa());
8314         assertNull(calibrator.getEstimatedSx());
8315         assertNull(calibrator.getEstimatedSy());
8316         assertNull(calibrator.getEstimatedSz());
8317         assertNull(calibrator.getEstimatedMxy());
8318         assertNull(calibrator.getEstimatedMxz());
8319         assertNull(calibrator.getEstimatedMyx());
8320         assertNull(calibrator.getEstimatedMyz());
8321         assertNull(calibrator.getEstimatedMzx());
8322         assertNull(calibrator.getEstimatedMzy());
8323         assertNull(calibrator.getEstimatedCovariance());
8324         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8325         assertNull(calibrator.getGroundTruthGravityNorm());
8326         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8327         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8328 
8329         // Force IllegalArgumentException
8330         calibrator = null;
8331         try {
8332             calibrator = new KnownGravityNormAccelerometerCalibrator(
8333                     measurements, new Matrix(1, 1), ma,
8334                     this);
8335             fail("IllegalArgumentException expected but not thrown");
8336         } catch (final IllegalArgumentException ignore) {
8337         }
8338         try {
8339             calibrator = new KnownGravityNormAccelerometerCalibrator(
8340                     measurements, new Matrix(1, 3), ma,
8341                     this);
8342             fail("IllegalArgumentException expected but not thrown");
8343         } catch (final IllegalArgumentException ignore) {
8344         }
8345         try {
8346             calibrator = new KnownGravityNormAccelerometerCalibrator(
8347                     measurements, ba, new Matrix(1, 3),
8348                     this);
8349             fail("IllegalArgumentException expected but not thrown");
8350         } catch (final IllegalArgumentException ignore) {
8351         }
8352         try {
8353             calibrator = new KnownGravityNormAccelerometerCalibrator(
8354                     measurements, ba, new Matrix(3, 1),
8355                     this);
8356             fail("IllegalArgumentException expected but not thrown");
8357         } catch (final IllegalArgumentException ignore) {
8358         }
8359         assertNull(calibrator);
8360     }
8361 
8362     @Test
8363     public void testConstructor75() throws WrongSizeException {
8364         final Matrix ba = generateBa();
8365         final double biasX = ba.getElementAtIndex(0);
8366         final double biasY = ba.getElementAtIndex(1);
8367         final double biasZ = ba.getElementAtIndex(2);
8368 
8369         final Matrix ma = generateMaCommonAxis();
8370         final double sx = ma.getElementAt(0, 0);
8371         final double sy = ma.getElementAt(1, 1);
8372         final double sz = ma.getElementAt(2, 2);
8373         final double mxy = ma.getElementAt(0, 1);
8374         final double mxz = ma.getElementAt(0, 2);
8375         final double myx = ma.getElementAt(1, 0);
8376         final double myz = ma.getElementAt(1, 2);
8377         final double mzx = ma.getElementAt(2, 0);
8378         final double mzy = ma.getElementAt(2, 1);
8379 
8380         KnownGravityNormAccelerometerCalibrator calibrator =
8381                 new KnownGravityNormAccelerometerCalibrator(true, ba, ma);
8382 
8383         // check default values
8384         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8385         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8386         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8387         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8388         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8389         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8390         final Acceleration bx2 = new Acceleration(0.0,
8391                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8392         calibrator.getInitialBiasXAsAcceleration(bx2);
8393         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8394         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8395         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8396         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8397         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8398         final Acceleration by2 = new Acceleration(0.0,
8399                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8400         calibrator.getInitialBiasYAsAcceleration(by2);
8401         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8402         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8403         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8404         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8405         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8406         final Acceleration bz2 = new Acceleration(0.0,
8407                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8408         calibrator.getInitialBiasZAsAcceleration(bz2);
8409         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8410         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8411         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8412         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8413         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8414         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8415         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8416         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8417         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8418         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8419         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8420         final double[] bias1 = calibrator.getInitialBias();
8421         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8422         final double[] bias2 = new double[3];
8423         calibrator.getInitialBias(bias2);
8424         assertArrayEquals(bias1, bias2, 0.0);
8425         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8426         assertEquals(b1, ba);
8427         final Matrix b2 = new Matrix(3, 1);
8428         calibrator.getInitialBiasAsMatrix(b2);
8429         assertEquals(b1, b2);
8430         final Matrix ma1 = new Matrix(3, 3);
8431         ma1.setSubmatrix(0, 0,
8432                 2, 2,
8433                 new double[]{sx, myx, mzx,
8434                         mxy, sy, mzy,
8435                         mxz, myz, sz});
8436         assertEquals(calibrator.getInitialMa(), ma1);
8437         final Matrix ma2 = new Matrix(3, 3);
8438         calibrator.getInitialMa(ma2);
8439         assertEquals(ma1, ma2);
8440         assertNull(calibrator.getMeasurements());
8441         assertTrue(calibrator.isCommonAxisUsed());
8442         assertNull(calibrator.getListener());
8443         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8444         assertFalse(calibrator.isReady());
8445         assertFalse(calibrator.isRunning());
8446         assertNull(calibrator.getEstimatedBiases());
8447         assertFalse(calibrator.getEstimatedBiases(null));
8448         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8449         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8450         assertNull(calibrator.getEstimatedBiasFx());
8451         assertNull(calibrator.getEstimatedBiasFy());
8452         assertNull(calibrator.getEstimatedBiasFz());
8453         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8454         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8455         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8456         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8457         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8458         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8459         assertNull(calibrator.getEstimatedMa());
8460         assertNull(calibrator.getEstimatedSx());
8461         assertNull(calibrator.getEstimatedSy());
8462         assertNull(calibrator.getEstimatedSz());
8463         assertNull(calibrator.getEstimatedMxy());
8464         assertNull(calibrator.getEstimatedMxz());
8465         assertNull(calibrator.getEstimatedMyx());
8466         assertNull(calibrator.getEstimatedMyz());
8467         assertNull(calibrator.getEstimatedMzx());
8468         assertNull(calibrator.getEstimatedMzy());
8469         assertNull(calibrator.getEstimatedCovariance());
8470         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8471         assertNull(calibrator.getGroundTruthGravityNorm());
8472         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8473         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8474 
8475         // Force IllegalArgumentException
8476         calibrator = null;
8477         try {
8478             calibrator = new KnownGravityNormAccelerometerCalibrator(
8479                     true, new Matrix(1, 1), ma);
8480             fail("IllegalArgumentException expected but not thrown");
8481         } catch (final IllegalArgumentException ignore) {
8482         }
8483         try {
8484             calibrator = new KnownGravityNormAccelerometerCalibrator(
8485                     true, new Matrix(1, 3), ma);
8486             fail("IllegalArgumentException expected but not thrown");
8487         } catch (final IllegalArgumentException ignore) {
8488         }
8489         try {
8490             calibrator = new KnownGravityNormAccelerometerCalibrator(
8491                     true, ba, new Matrix(1, 3));
8492             fail("IllegalArgumentException expected but not thrown");
8493         } catch (final IllegalArgumentException ignore) {
8494         }
8495         try {
8496             calibrator = new KnownGravityNormAccelerometerCalibrator(
8497                     true, ba, new Matrix(3, 1));
8498             fail("IllegalArgumentException expected but not thrown");
8499         } catch (final IllegalArgumentException ignore) {
8500         }
8501         assertNull(calibrator);
8502     }
8503 
8504     @Test
8505     public void testConstructor76() throws WrongSizeException {
8506         final Matrix ba = generateBa();
8507         final double biasX = ba.getElementAtIndex(0);
8508         final double biasY = ba.getElementAtIndex(1);
8509         final double biasZ = ba.getElementAtIndex(2);
8510 
8511         final Matrix ma = generateMaCommonAxis();
8512         final double sx = ma.getElementAt(0, 0);
8513         final double sy = ma.getElementAt(1, 1);
8514         final double sz = ma.getElementAt(2, 2);
8515         final double mxy = ma.getElementAt(0, 1);
8516         final double mxz = ma.getElementAt(0, 2);
8517         final double myx = ma.getElementAt(1, 0);
8518         final double myz = ma.getElementAt(1, 2);
8519         final double mzx = ma.getElementAt(2, 0);
8520         final double mzy = ma.getElementAt(2, 1);
8521 
8522         KnownGravityNormAccelerometerCalibrator calibrator =
8523                 new KnownGravityNormAccelerometerCalibrator(
8524                         true, ba, ma, this);
8525 
8526         // check default values
8527         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8528         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8529         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8530         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8531         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8532         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8533         final Acceleration bx2 = new Acceleration(0.0,
8534                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8535         calibrator.getInitialBiasXAsAcceleration(bx2);
8536         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8537         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8538         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8539         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8540         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8541         final Acceleration by2 = new Acceleration(0.0,
8542                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8543         calibrator.getInitialBiasYAsAcceleration(by2);
8544         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8545         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8546         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8547         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8548         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8549         final Acceleration bz2 = new Acceleration(0.0,
8550                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8551         calibrator.getInitialBiasZAsAcceleration(bz2);
8552         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8553         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8554         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8555         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8556         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8557         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8558         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8559         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8560         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8561         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8562         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8563         final double[] bias1 = calibrator.getInitialBias();
8564         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8565         final double[] bias2 = new double[3];
8566         calibrator.getInitialBias(bias2);
8567         assertArrayEquals(bias1, bias2, 0.0);
8568         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8569         assertEquals(b1, ba);
8570         final Matrix b2 = new Matrix(3, 1);
8571         calibrator.getInitialBiasAsMatrix(b2);
8572         assertEquals(b1, b2);
8573         final Matrix ma1 = new Matrix(3, 3);
8574         ma1.setSubmatrix(0, 0,
8575                 2, 2,
8576                 new double[]{sx, myx, mzx,
8577                         mxy, sy, mzy,
8578                         mxz, myz, sz});
8579         assertEquals(calibrator.getInitialMa(), ma1);
8580         final Matrix ma2 = new Matrix(3, 3);
8581         calibrator.getInitialMa(ma2);
8582         assertEquals(ma1, ma2);
8583         assertNull(calibrator.getMeasurements());
8584         assertTrue(calibrator.isCommonAxisUsed());
8585         assertSame(calibrator.getListener(), this);
8586         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8587         assertFalse(calibrator.isReady());
8588         assertFalse(calibrator.isRunning());
8589         assertNull(calibrator.getEstimatedBiases());
8590         assertFalse(calibrator.getEstimatedBiases(null));
8591         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8592         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8593         assertNull(calibrator.getEstimatedBiasFx());
8594         assertNull(calibrator.getEstimatedBiasFy());
8595         assertNull(calibrator.getEstimatedBiasFz());
8596         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8597         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8598         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8599         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8600         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8601         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8602         assertNull(calibrator.getEstimatedMa());
8603         assertNull(calibrator.getEstimatedSx());
8604         assertNull(calibrator.getEstimatedSy());
8605         assertNull(calibrator.getEstimatedSz());
8606         assertNull(calibrator.getEstimatedMxy());
8607         assertNull(calibrator.getEstimatedMxz());
8608         assertNull(calibrator.getEstimatedMyx());
8609         assertNull(calibrator.getEstimatedMyz());
8610         assertNull(calibrator.getEstimatedMzx());
8611         assertNull(calibrator.getEstimatedMzy());
8612         assertNull(calibrator.getEstimatedCovariance());
8613         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8614         assertNull(calibrator.getGroundTruthGravityNorm());
8615         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8616         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8617 
8618         // Force IllegalArgumentException
8619         calibrator = null;
8620         try {
8621             calibrator = new KnownGravityNormAccelerometerCalibrator(
8622                     true, new Matrix(1, 1), ma,
8623                     this);
8624             fail("IllegalArgumentException expected but not thrown");
8625         } catch (final IllegalArgumentException ignore) {
8626         }
8627         try {
8628             calibrator = new KnownGravityNormAccelerometerCalibrator(
8629                     true, new Matrix(1, 3), ma,
8630                     this);
8631             fail("IllegalArgumentException expected but not thrown");
8632         } catch (final IllegalArgumentException ignore) {
8633         }
8634         try {
8635             calibrator = new KnownGravityNormAccelerometerCalibrator(
8636                     true, ba, new Matrix(1, 3),
8637                     this);
8638             fail("IllegalArgumentException expected but not thrown");
8639         } catch (final IllegalArgumentException ignore) {
8640         }
8641         try {
8642             calibrator = new KnownGravityNormAccelerometerCalibrator(
8643                     true, ba, new Matrix(3, 1),
8644                     this);
8645             fail("IllegalArgumentException expected but not thrown");
8646         } catch (final IllegalArgumentException ignore) {
8647         }
8648         assertNull(calibrator);
8649     }
8650 
8651     @Test
8652     public void testConstructor77() throws WrongSizeException {
8653         final Collection<StandardDeviationBodyKinematics> measurements =
8654                 Collections.emptyList();
8655 
8656         final Matrix ba = generateBa();
8657         final double biasX = ba.getElementAtIndex(0);
8658         final double biasY = ba.getElementAtIndex(1);
8659         final double biasZ = ba.getElementAtIndex(2);
8660 
8661         final Matrix ma = generateMaCommonAxis();
8662         final double sx = ma.getElementAt(0, 0);
8663         final double sy = ma.getElementAt(1, 1);
8664         final double sz = ma.getElementAt(2, 2);
8665         final double mxy = ma.getElementAt(0, 1);
8666         final double mxz = ma.getElementAt(0, 2);
8667         final double myx = ma.getElementAt(1, 0);
8668         final double myz = ma.getElementAt(1, 2);
8669         final double mzx = ma.getElementAt(2, 0);
8670         final double mzy = ma.getElementAt(2, 1);
8671 
8672         KnownGravityNormAccelerometerCalibrator calibrator =
8673                 new KnownGravityNormAccelerometerCalibrator(measurements,
8674                         true, ba, ma);
8675 
8676         // check default values
8677         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8678         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8679         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8680         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8681         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8682         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8683         final Acceleration bx2 = new Acceleration(0.0,
8684                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8685         calibrator.getInitialBiasXAsAcceleration(bx2);
8686         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8687         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8688         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8689         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8690         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8691         final Acceleration by2 = new Acceleration(0.0,
8692                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8693         calibrator.getInitialBiasYAsAcceleration(by2);
8694         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8695         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8696         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8697         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8698         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8699         final Acceleration bz2 = new Acceleration(0.0,
8700                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8701         calibrator.getInitialBiasZAsAcceleration(bz2);
8702         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8703         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8704         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8705         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8706         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8707         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8708         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8709         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8710         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8711         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8712         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8713         final double[] bias1 = calibrator.getInitialBias();
8714         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8715         final double[] bias2 = new double[3];
8716         calibrator.getInitialBias(bias2);
8717         assertArrayEquals(bias1, bias2, 0.0);
8718         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8719         assertEquals(b1, ba);
8720         final Matrix b2 = new Matrix(3, 1);
8721         calibrator.getInitialBiasAsMatrix(b2);
8722         assertEquals(b1, b2);
8723         final Matrix ma1 = new Matrix(3, 3);
8724         ma1.setSubmatrix(0, 0,
8725                 2, 2,
8726                 new double[]{sx, myx, mzx,
8727                         mxy, sy, mzy,
8728                         mxz, myz, sz});
8729         assertEquals(calibrator.getInitialMa(), ma1);
8730         final Matrix ma2 = new Matrix(3, 3);
8731         calibrator.getInitialMa(ma2);
8732         assertEquals(ma1, ma2);
8733         assertSame(calibrator.getMeasurements(), measurements);
8734         assertTrue(calibrator.isCommonAxisUsed());
8735         assertNull(calibrator.getListener());
8736         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8737         assertFalse(calibrator.isReady());
8738         assertFalse(calibrator.isRunning());
8739         assertNull(calibrator.getEstimatedBiases());
8740         assertFalse(calibrator.getEstimatedBiases(null));
8741         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8742         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8743         assertNull(calibrator.getEstimatedBiasFx());
8744         assertNull(calibrator.getEstimatedBiasFy());
8745         assertNull(calibrator.getEstimatedBiasFz());
8746         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8747         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8748         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8749         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8750         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8751         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8752         assertNull(calibrator.getEstimatedMa());
8753         assertNull(calibrator.getEstimatedSx());
8754         assertNull(calibrator.getEstimatedSy());
8755         assertNull(calibrator.getEstimatedSz());
8756         assertNull(calibrator.getEstimatedMxy());
8757         assertNull(calibrator.getEstimatedMxz());
8758         assertNull(calibrator.getEstimatedMyx());
8759         assertNull(calibrator.getEstimatedMyz());
8760         assertNull(calibrator.getEstimatedMzx());
8761         assertNull(calibrator.getEstimatedMzy());
8762         assertNull(calibrator.getEstimatedCovariance());
8763         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8764         assertNull(calibrator.getGroundTruthGravityNorm());
8765         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8766         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8767 
8768         // Force IllegalArgumentException
8769         calibrator = null;
8770         try {
8771             calibrator = new KnownGravityNormAccelerometerCalibrator(
8772                     measurements, true,
8773                     new Matrix(1, 1), ma);
8774             fail("IllegalArgumentException expected but not thrown");
8775         } catch (final IllegalArgumentException ignore) {
8776         }
8777         try {
8778             calibrator = new KnownGravityNormAccelerometerCalibrator(
8779                     measurements, true,
8780                     new Matrix(1, 3), ma);
8781             fail("IllegalArgumentException expected but not thrown");
8782         } catch (final IllegalArgumentException ignore) {
8783         }
8784         try {
8785             calibrator = new KnownGravityNormAccelerometerCalibrator(
8786                     measurements, true, ba,
8787                     new Matrix(1, 3));
8788             fail("IllegalArgumentException expected but not thrown");
8789         } catch (final IllegalArgumentException ignore) {
8790         }
8791         try {
8792             calibrator = new KnownGravityNormAccelerometerCalibrator(
8793                     measurements, true, ba,
8794                     new Matrix(3, 1));
8795             fail("IllegalArgumentException expected but not thrown");
8796         } catch (final IllegalArgumentException ignore) {
8797         }
8798         assertNull(calibrator);
8799     }
8800 
8801     @Test
8802     public void testConstructor78() throws WrongSizeException {
8803         final Collection<StandardDeviationBodyKinematics> measurements =
8804                 Collections.emptyList();
8805 
8806         final Matrix ba = generateBa();
8807         final double biasX = ba.getElementAtIndex(0);
8808         final double biasY = ba.getElementAtIndex(1);
8809         final double biasZ = ba.getElementAtIndex(2);
8810 
8811         final Matrix ma = generateMaCommonAxis();
8812         final double sx = ma.getElementAt(0, 0);
8813         final double sy = ma.getElementAt(1, 1);
8814         final double sz = ma.getElementAt(2, 2);
8815         final double mxy = ma.getElementAt(0, 1);
8816         final double mxz = ma.getElementAt(0, 2);
8817         final double myx = ma.getElementAt(1, 0);
8818         final double myz = ma.getElementAt(1, 2);
8819         final double mzx = ma.getElementAt(2, 0);
8820         final double mzy = ma.getElementAt(2, 1);
8821 
8822         KnownGravityNormAccelerometerCalibrator calibrator =
8823                 new KnownGravityNormAccelerometerCalibrator(measurements,
8824                         true, ba, ma, this);
8825 
8826         // check default values
8827         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8828         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8829         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8830         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8831         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8832         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8833         final Acceleration bx2 = new Acceleration(0.0,
8834                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8835         calibrator.getInitialBiasXAsAcceleration(bx2);
8836         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8837         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8838         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8839         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8840         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8841         final Acceleration by2 = new Acceleration(0.0,
8842                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8843         calibrator.getInitialBiasYAsAcceleration(by2);
8844         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8845         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8846         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8847         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8848         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8849         final Acceleration bz2 = new Acceleration(0.0,
8850                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8851         calibrator.getInitialBiasZAsAcceleration(bz2);
8852         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8853         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8854         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8855         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8856         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8857         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8858         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8859         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8860         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8861         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8862         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8863         final double[] bias1 = calibrator.getInitialBias();
8864         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8865         final double[] bias2 = new double[3];
8866         calibrator.getInitialBias(bias2);
8867         assertArrayEquals(bias1, bias2, 0.0);
8868         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8869         assertEquals(b1, ba);
8870         final Matrix b2 = new Matrix(3, 1);
8871         calibrator.getInitialBiasAsMatrix(b2);
8872         assertEquals(b1, b2);
8873         final Matrix ma1 = new Matrix(3, 3);
8874         ma1.setSubmatrix(0, 0,
8875                 2, 2,
8876                 new double[]{sx, myx, mzx,
8877                         mxy, sy, mzy,
8878                         mxz, myz, sz});
8879         assertEquals(calibrator.getInitialMa(), ma1);
8880         final Matrix ma2 = new Matrix(3, 3);
8881         calibrator.getInitialMa(ma2);
8882         assertEquals(ma1, ma2);
8883         assertSame(calibrator.getMeasurements(), measurements);
8884         assertTrue(calibrator.isCommonAxisUsed());
8885         assertSame(calibrator.getListener(), this);
8886         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8887         assertFalse(calibrator.isReady());
8888         assertFalse(calibrator.isRunning());
8889         assertNull(calibrator.getEstimatedBiases());
8890         assertFalse(calibrator.getEstimatedBiases(null));
8891         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8892         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8893         assertNull(calibrator.getEstimatedBiasFx());
8894         assertNull(calibrator.getEstimatedBiasFy());
8895         assertNull(calibrator.getEstimatedBiasFz());
8896         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8897         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8898         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8899         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8900         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8901         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8902         assertNull(calibrator.getEstimatedMa());
8903         assertNull(calibrator.getEstimatedSx());
8904         assertNull(calibrator.getEstimatedSy());
8905         assertNull(calibrator.getEstimatedSz());
8906         assertNull(calibrator.getEstimatedMxy());
8907         assertNull(calibrator.getEstimatedMxz());
8908         assertNull(calibrator.getEstimatedMyx());
8909         assertNull(calibrator.getEstimatedMyz());
8910         assertNull(calibrator.getEstimatedMzx());
8911         assertNull(calibrator.getEstimatedMzy());
8912         assertNull(calibrator.getEstimatedCovariance());
8913         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8914         assertNull(calibrator.getGroundTruthGravityNorm());
8915         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8916         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8917 
8918         // Force IllegalArgumentException
8919         calibrator = null;
8920         try {
8921             calibrator = new KnownGravityNormAccelerometerCalibrator(
8922                     measurements, true,
8923                     new Matrix(1, 1), ma,
8924                     this);
8925             fail("IllegalArgumentException expected but not thrown");
8926         } catch (final IllegalArgumentException ignore) {
8927         }
8928         try {
8929             calibrator = new KnownGravityNormAccelerometerCalibrator(
8930                     measurements, true,
8931                     new Matrix(1, 3), ma,
8932                     this);
8933             fail("IllegalArgumentException expected but not thrown");
8934         } catch (final IllegalArgumentException ignore) {
8935         }
8936         try {
8937             calibrator = new KnownGravityNormAccelerometerCalibrator(
8938                     measurements, true, ba, new Matrix(1, 3),
8939                     this);
8940             fail("IllegalArgumentException expected but not thrown");
8941         } catch (final IllegalArgumentException ignore) {
8942         }
8943         try {
8944             calibrator = new KnownGravityNormAccelerometerCalibrator(
8945                     measurements, true, ba, new Matrix(3, 1),
8946                     this);
8947             fail("IllegalArgumentException expected but not thrown");
8948         } catch (final IllegalArgumentException ignore) {
8949         }
8950         assertNull(calibrator);
8951     }
8952 
8953     @Test
8954     public void testConstructor79() throws WrongSizeException {
8955         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
8956         final double latitude = Math.toRadians(
8957                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8958         final double longitude = Math.toRadians(
8959                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8960         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8961         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
8962         final NEDVelocity nedVelocity = new NEDVelocity();
8963         final ECEFPosition ecefPosition = new ECEFPosition();
8964         final ECEFVelocity ecefVelocity = new ECEFVelocity();
8965         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
8966                 ecefPosition, ecefVelocity);
8967         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8968                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8969         final double gravityNorm = gravity.getNorm();
8970 
8971         KnownGravityNormAccelerometerCalibrator calibrator =
8972                 new KnownGravityNormAccelerometerCalibrator(gravityNorm);
8973 
8974         // check default values
8975         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
8976         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
8977         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
8978         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8979         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
8980         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8981         final Acceleration bx2 = new Acceleration(0.0,
8982                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8983         calibrator.getInitialBiasXAsAcceleration(bx2);
8984         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
8985         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8986         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8987         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
8988         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8989         final Acceleration by2 = new Acceleration(0.0,
8990                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8991         calibrator.getInitialBiasYAsAcceleration(by2);
8992         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
8993         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8994         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8995         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
8996         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8997         final Acceleration bz2 = new Acceleration(0.0,
8998                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8999         calibrator.getInitialBiasZAsAcceleration(bz2);
9000         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9001         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9002         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9003         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9004         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9005         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9006         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9007         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9008         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9009         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9010         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9011         final double[] bias1 = calibrator.getInitialBias();
9012         assertArrayEquals(bias1, new double[3], 0.0);
9013         final double[] bias2 = new double[3];
9014         calibrator.getInitialBias(bias2);
9015         assertArrayEquals(bias1, bias2, 0.0);
9016         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9017         assertEquals(b1, new Matrix(3, 1));
9018         final Matrix b2 = new Matrix(3, 1);
9019         calibrator.getInitialBiasAsMatrix(b2);
9020         assertEquals(b1, b2);
9021         final Matrix ma1 = calibrator.getInitialMa();
9022         assertEquals(ma1, new Matrix(3, 3));
9023         final Matrix ma2 = new Matrix(3, 3);
9024         calibrator.getInitialMa(ma2);
9025         assertEquals(ma1, ma2);
9026         assertNull(calibrator.getMeasurements());
9027         assertFalse(calibrator.isCommonAxisUsed());
9028         assertNull(calibrator.getListener());
9029         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9030         assertFalse(calibrator.isReady());
9031         assertFalse(calibrator.isRunning());
9032         assertNull(calibrator.getEstimatedBiases());
9033         assertFalse(calibrator.getEstimatedBiases(null));
9034         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9035         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9036         assertNull(calibrator.getEstimatedBiasFx());
9037         assertNull(calibrator.getEstimatedBiasFy());
9038         assertNull(calibrator.getEstimatedBiasFz());
9039         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9040         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9041         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9042         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9043         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9044         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9045         assertNull(calibrator.getEstimatedMa());
9046         assertNull(calibrator.getEstimatedSx());
9047         assertNull(calibrator.getEstimatedSy());
9048         assertNull(calibrator.getEstimatedSz());
9049         assertNull(calibrator.getEstimatedMxy());
9050         assertNull(calibrator.getEstimatedMxz());
9051         assertNull(calibrator.getEstimatedMyx());
9052         assertNull(calibrator.getEstimatedMyz());
9053         assertNull(calibrator.getEstimatedMzx());
9054         assertNull(calibrator.getEstimatedMzy());
9055         assertNull(calibrator.getEstimatedCovariance());
9056         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9057         assertNotNull(calibrator.getGroundTruthGravityNorm());
9058         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9059         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9060         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9061                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9062         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9063         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9064         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9065 
9066         // Force IllegalArgumentException
9067         calibrator = null;
9068         try {
9069             calibrator = new KnownGravityNormAccelerometerCalibrator(
9070                     -gravityNorm);
9071             fail("IllegalArgumentException expected but not thrown");
9072         } catch (final IllegalArgumentException ignore) {
9073         }
9074         assertNull(calibrator);
9075     }
9076 
9077     @Test
9078     public void testConstructor80() throws WrongSizeException {
9079         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9080         final double latitude = Math.toRadians(
9081                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9082         final double longitude = Math.toRadians(
9083                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9084         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9085         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9086         final NEDVelocity nedVelocity = new NEDVelocity();
9087         final ECEFPosition ecefPosition = new ECEFPosition();
9088         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9089         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9090                 ecefPosition, ecefVelocity);
9091         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9092                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9093         final double gravityNorm = gravity.getNorm();
9094 
9095         KnownGravityNormAccelerometerCalibrator calibrator =
9096                 new KnownGravityNormAccelerometerCalibrator(
9097                         gravityNorm, this);
9098 
9099         // check default values
9100         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9101         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9102         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9103         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9104         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9105         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9106         final Acceleration bx2 = new Acceleration(0.0,
9107                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9108         calibrator.getInitialBiasXAsAcceleration(bx2);
9109         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9110         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9111         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9112         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9113         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9114         final Acceleration by2 = new Acceleration(0.0,
9115                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9116         calibrator.getInitialBiasYAsAcceleration(by2);
9117         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9118         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9119         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9120         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9121         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9122         final Acceleration bz2 = new Acceleration(0.0,
9123                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9124         calibrator.getInitialBiasZAsAcceleration(bz2);
9125         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9126         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9127         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9128         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9129         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9130         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9131         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9132         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9133         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9134         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9135         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9136         final double[] bias1 = calibrator.getInitialBias();
9137         assertArrayEquals(bias1, new double[3], 0.0);
9138         final double[] bias2 = new double[3];
9139         calibrator.getInitialBias(bias2);
9140         assertArrayEquals(bias1, bias2, 0.0);
9141         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9142         assertEquals(b1, new Matrix(3, 1));
9143         final Matrix b2 = new Matrix(3, 1);
9144         calibrator.getInitialBiasAsMatrix(b2);
9145         assertEquals(b1, b2);
9146         final Matrix ma1 = calibrator.getInitialMa();
9147         assertEquals(ma1, new Matrix(3, 3));
9148         final Matrix ma2 = new Matrix(3, 3);
9149         calibrator.getInitialMa(ma2);
9150         assertEquals(ma1, ma2);
9151         assertNull(calibrator.getMeasurements());
9152         assertFalse(calibrator.isCommonAxisUsed());
9153         assertSame(calibrator.getListener(), this);
9154         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9155         assertFalse(calibrator.isReady());
9156         assertFalse(calibrator.isRunning());
9157         assertNull(calibrator.getEstimatedBiases());
9158         assertFalse(calibrator.getEstimatedBiases(null));
9159         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9160         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9161         assertNull(calibrator.getEstimatedBiasFx());
9162         assertNull(calibrator.getEstimatedBiasFy());
9163         assertNull(calibrator.getEstimatedBiasFz());
9164         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9165         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9166         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9167         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9168         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9169         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9170         assertNull(calibrator.getEstimatedMa());
9171         assertNull(calibrator.getEstimatedSx());
9172         assertNull(calibrator.getEstimatedSy());
9173         assertNull(calibrator.getEstimatedSz());
9174         assertNull(calibrator.getEstimatedMxy());
9175         assertNull(calibrator.getEstimatedMxz());
9176         assertNull(calibrator.getEstimatedMyx());
9177         assertNull(calibrator.getEstimatedMyz());
9178         assertNull(calibrator.getEstimatedMzx());
9179         assertNull(calibrator.getEstimatedMzy());
9180         assertNull(calibrator.getEstimatedCovariance());
9181         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9182         assertNotNull(calibrator.getGroundTruthGravityNorm());
9183         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9184         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9185         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9186                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9187         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9188         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9189         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9190 
9191         // Force IllegalArgumentException
9192         calibrator = null;
9193         try {
9194             calibrator = new KnownGravityNormAccelerometerCalibrator(
9195                     -gravityNorm, this);
9196             fail("IllegalArgumentException expected but not thrown");
9197         } catch (final IllegalArgumentException ignore) {
9198         }
9199         assertNull(calibrator);
9200     }
9201 
9202     @Test
9203     public void testConstructor81() throws WrongSizeException {
9204         final Collection<StandardDeviationBodyKinematics> measurements =
9205                 Collections.emptyList();
9206 
9207         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9208         final double latitude = Math.toRadians(
9209                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9210         final double longitude = Math.toRadians(
9211                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9212         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9213         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9214         final NEDVelocity nedVelocity = new NEDVelocity();
9215         final ECEFPosition ecefPosition = new ECEFPosition();
9216         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9217         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9218                 ecefPosition, ecefVelocity);
9219         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9220                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9221         final double gravityNorm = gravity.getNorm();
9222 
9223         KnownGravityNormAccelerometerCalibrator calibrator =
9224                 new KnownGravityNormAccelerometerCalibrator(
9225                         gravityNorm, measurements);
9226 
9227         // check default values
9228         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9229         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9230         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9231         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9232         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9233         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9234         final Acceleration bx2 = new Acceleration(0.0,
9235                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9236         calibrator.getInitialBiasXAsAcceleration(bx2);
9237         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9238         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9239         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9240         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9241         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9242         final Acceleration by2 = new Acceleration(0.0,
9243                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9244         calibrator.getInitialBiasYAsAcceleration(by2);
9245         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9246         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9247         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9248         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9249         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9250         final Acceleration bz2 = new Acceleration(0.0,
9251                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9252         calibrator.getInitialBiasZAsAcceleration(bz2);
9253         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9254         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9255         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9256         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9257         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9258         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9259         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9260         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9261         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9262         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9263         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9264         final double[] bias1 = calibrator.getInitialBias();
9265         assertArrayEquals(bias1, new double[3], 0.0);
9266         final double[] bias2 = new double[3];
9267         calibrator.getInitialBias(bias2);
9268         assertArrayEquals(bias1, bias2, 0.0);
9269         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9270         assertEquals(b1, new Matrix(3, 1));
9271         final Matrix b2 = new Matrix(3, 1);
9272         calibrator.getInitialBiasAsMatrix(b2);
9273         assertEquals(b1, b2);
9274         final Matrix ma1 = calibrator.getInitialMa();
9275         assertEquals(ma1, new Matrix(3, 3));
9276         final Matrix ma2 = new Matrix(3, 3);
9277         calibrator.getInitialMa(ma2);
9278         assertEquals(ma1, ma2);
9279         assertSame(calibrator.getMeasurements(), measurements);
9280         assertFalse(calibrator.isCommonAxisUsed());
9281         assertNull(calibrator.getListener());
9282         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9283         assertFalse(calibrator.isReady());
9284         assertFalse(calibrator.isRunning());
9285         assertNull(calibrator.getEstimatedBiases());
9286         assertFalse(calibrator.getEstimatedBiases(null));
9287         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9288         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9289         assertNull(calibrator.getEstimatedBiasFx());
9290         assertNull(calibrator.getEstimatedBiasFy());
9291         assertNull(calibrator.getEstimatedBiasFz());
9292         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9293         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9294         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9295         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9296         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9297         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9298         assertNull(calibrator.getEstimatedMa());
9299         assertNull(calibrator.getEstimatedSx());
9300         assertNull(calibrator.getEstimatedSy());
9301         assertNull(calibrator.getEstimatedSz());
9302         assertNull(calibrator.getEstimatedMxy());
9303         assertNull(calibrator.getEstimatedMxz());
9304         assertNull(calibrator.getEstimatedMyx());
9305         assertNull(calibrator.getEstimatedMyz());
9306         assertNull(calibrator.getEstimatedMzx());
9307         assertNull(calibrator.getEstimatedMzy());
9308         assertNull(calibrator.getEstimatedCovariance());
9309         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9310         assertNotNull(calibrator.getGroundTruthGravityNorm());
9311         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9312         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9313         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9314                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9315         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9316         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9317         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9318 
9319         // Force IllegalArgumentException
9320         calibrator = null;
9321         try {
9322             calibrator = new KnownGravityNormAccelerometerCalibrator(
9323                     -gravityNorm, measurements);
9324             fail("IllegalArgumentException expected but not thrown");
9325         } catch (final IllegalArgumentException ignore) {
9326         }
9327         assertNull(calibrator);
9328     }
9329 
9330     @Test
9331     public void testConstructor82() throws WrongSizeException {
9332         final Collection<StandardDeviationBodyKinematics> measurements =
9333                 Collections.emptyList();
9334 
9335         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9336         final double latitude = Math.toRadians(
9337                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9338         final double longitude = Math.toRadians(
9339                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9340         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9341         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9342         final NEDVelocity nedVelocity = new NEDVelocity();
9343         final ECEFPosition ecefPosition = new ECEFPosition();
9344         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9345         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9346                 ecefPosition, ecefVelocity);
9347         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9348                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9349         final double gravityNorm = gravity.getNorm();
9350 
9351         KnownGravityNormAccelerometerCalibrator calibrator =
9352                 new KnownGravityNormAccelerometerCalibrator(
9353                         gravityNorm, measurements, this);
9354 
9355         // check default values
9356         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9357         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9358         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9359         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9360         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9361         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9362         final Acceleration bx2 = new Acceleration(0.0,
9363                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9364         calibrator.getInitialBiasXAsAcceleration(bx2);
9365         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9366         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9367         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9368         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9369         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9370         final Acceleration by2 = new Acceleration(0.0,
9371                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9372         calibrator.getInitialBiasYAsAcceleration(by2);
9373         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9374         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9375         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9376         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9377         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9378         final Acceleration bz2 = new Acceleration(0.0,
9379                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9380         calibrator.getInitialBiasZAsAcceleration(bz2);
9381         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9382         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9383         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9384         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9385         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9386         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9387         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9388         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9389         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9390         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9391         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9392         final double[] bias1 = calibrator.getInitialBias();
9393         assertArrayEquals(bias1, new double[3], 0.0);
9394         final double[] bias2 = new double[3];
9395         calibrator.getInitialBias(bias2);
9396         assertArrayEquals(bias1, bias2, 0.0);
9397         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9398         assertEquals(b1, new Matrix(3, 1));
9399         final Matrix b2 = new Matrix(3, 1);
9400         calibrator.getInitialBiasAsMatrix(b2);
9401         assertEquals(b1, b2);
9402         final Matrix ma1 = calibrator.getInitialMa();
9403         assertEquals(ma1, new Matrix(3, 3));
9404         final Matrix ma2 = new Matrix(3, 3);
9405         calibrator.getInitialMa(ma2);
9406         assertEquals(ma1, ma2);
9407         assertSame(calibrator.getMeasurements(), measurements);
9408         assertFalse(calibrator.isCommonAxisUsed());
9409         assertSame(calibrator.getListener(), this);
9410         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9411         assertFalse(calibrator.isReady());
9412         assertFalse(calibrator.isRunning());
9413         assertNull(calibrator.getEstimatedBiases());
9414         assertFalse(calibrator.getEstimatedBiases(null));
9415         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9416         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9417         assertNull(calibrator.getEstimatedBiasFx());
9418         assertNull(calibrator.getEstimatedBiasFy());
9419         assertNull(calibrator.getEstimatedBiasFz());
9420         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9421         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9422         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9423         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9424         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9425         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9426         assertNull(calibrator.getEstimatedMa());
9427         assertNull(calibrator.getEstimatedSx());
9428         assertNull(calibrator.getEstimatedSy());
9429         assertNull(calibrator.getEstimatedSz());
9430         assertNull(calibrator.getEstimatedMxy());
9431         assertNull(calibrator.getEstimatedMxz());
9432         assertNull(calibrator.getEstimatedMyx());
9433         assertNull(calibrator.getEstimatedMyz());
9434         assertNull(calibrator.getEstimatedMzx());
9435         assertNull(calibrator.getEstimatedMzy());
9436         assertNull(calibrator.getEstimatedCovariance());
9437         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9438         assertNotNull(calibrator.getGroundTruthGravityNorm());
9439         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9440         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9441         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9442                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9443         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9444         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9445         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9446 
9447         // Force IllegalArgumentException
9448         calibrator = null;
9449         try {
9450             calibrator = new KnownGravityNormAccelerometerCalibrator(
9451                     -gravityNorm, measurements, this);
9452             fail("IllegalArgumentException expected but not thrown");
9453         } catch (final IllegalArgumentException ignore) {
9454         }
9455         assertNull(calibrator);
9456     }
9457 
9458     @Test
9459     public void testConstructor83() throws WrongSizeException {
9460         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9461         final double latitude = Math.toRadians(
9462                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9463         final double longitude = Math.toRadians(
9464                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9465         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9466         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9467         final NEDVelocity nedVelocity = new NEDVelocity();
9468         final ECEFPosition ecefPosition = new ECEFPosition();
9469         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9470         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9471                 ecefPosition, ecefVelocity);
9472         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9473                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9474         final double gravityNorm = gravity.getNorm();
9475 
9476         KnownGravityNormAccelerometerCalibrator calibrator =
9477                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
9478                         true);
9479 
9480         // check default values
9481         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9482         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9483         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9484         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9485         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9486         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9487         final Acceleration bx2 = new Acceleration(0.0,
9488                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9489         calibrator.getInitialBiasXAsAcceleration(bx2);
9490         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9491         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9492         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9493         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9494         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9495         final Acceleration by2 = new Acceleration(0.0,
9496                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9497         calibrator.getInitialBiasYAsAcceleration(by2);
9498         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9499         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9500         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9501         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9502         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9503         final Acceleration bz2 = new Acceleration(0.0,
9504                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9505         calibrator.getInitialBiasZAsAcceleration(bz2);
9506         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9507         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9508         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9509         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9510         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9511         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9512         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9513         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9514         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9515         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9516         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9517         final double[] bias1 = calibrator.getInitialBias();
9518         assertArrayEquals(bias1, new double[3], 0.0);
9519         final double[] bias2 = new double[3];
9520         calibrator.getInitialBias(bias2);
9521         assertArrayEquals(bias1, bias2, 0.0);
9522         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9523         assertEquals(b1, new Matrix(3, 1));
9524         final Matrix b2 = new Matrix(3, 1);
9525         calibrator.getInitialBiasAsMatrix(b2);
9526         assertEquals(b1, b2);
9527         final Matrix ma1 = calibrator.getInitialMa();
9528         assertEquals(ma1, new Matrix(3, 3));
9529         final Matrix ma2 = new Matrix(3, 3);
9530         calibrator.getInitialMa(ma2);
9531         assertEquals(ma1, ma2);
9532         assertNull(calibrator.getMeasurements());
9533         assertTrue(calibrator.isCommonAxisUsed());
9534         assertNull(calibrator.getListener());
9535         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9536         assertFalse(calibrator.isReady());
9537         assertFalse(calibrator.isRunning());
9538         assertNull(calibrator.getEstimatedBiases());
9539         assertFalse(calibrator.getEstimatedBiases(null));
9540         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9541         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9542         assertNull(calibrator.getEstimatedBiasFx());
9543         assertNull(calibrator.getEstimatedBiasFy());
9544         assertNull(calibrator.getEstimatedBiasFz());
9545         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9546         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9547         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9548         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9549         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9550         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9551         assertNull(calibrator.getEstimatedMa());
9552         assertNull(calibrator.getEstimatedSx());
9553         assertNull(calibrator.getEstimatedSy());
9554         assertNull(calibrator.getEstimatedSz());
9555         assertNull(calibrator.getEstimatedMxy());
9556         assertNull(calibrator.getEstimatedMxz());
9557         assertNull(calibrator.getEstimatedMyx());
9558         assertNull(calibrator.getEstimatedMyz());
9559         assertNull(calibrator.getEstimatedMzx());
9560         assertNull(calibrator.getEstimatedMzy());
9561         assertNull(calibrator.getEstimatedCovariance());
9562         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9563         assertNotNull(calibrator.getGroundTruthGravityNorm());
9564         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9565         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9566         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9567                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9568         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9569         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9570         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9571 
9572         // Force IllegalArgumentException
9573         calibrator = null;
9574         try {
9575             calibrator = new KnownGravityNormAccelerometerCalibrator(
9576                     -gravityNorm, true);
9577             fail("IllegalArgumentException expected but not thrown");
9578         } catch (final IllegalArgumentException ignore) {
9579         }
9580         assertNull(calibrator);
9581     }
9582 
9583     @Test
9584     public void testConstructor84() throws WrongSizeException {
9585         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9586         final double latitude = Math.toRadians(
9587                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9588         final double longitude = Math.toRadians(
9589                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9590         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9591         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9592         final NEDVelocity nedVelocity = new NEDVelocity();
9593         final ECEFPosition ecefPosition = new ECEFPosition();
9594         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9595         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9596                 ecefPosition, ecefVelocity);
9597         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9598                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9599         final double gravityNorm = gravity.getNorm();
9600 
9601         KnownGravityNormAccelerometerCalibrator calibrator =
9602                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
9603                         true, this);
9604 
9605         // check default values
9606         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9607         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9608         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9609         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9610         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9611         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9612         final Acceleration bx2 = new Acceleration(0.0,
9613                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9614         calibrator.getInitialBiasXAsAcceleration(bx2);
9615         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9616         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9617         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9618         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9619         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9620         final Acceleration by2 = new Acceleration(0.0,
9621                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9622         calibrator.getInitialBiasYAsAcceleration(by2);
9623         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9624         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9625         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9626         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9627         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9628         final Acceleration bz2 = new Acceleration(0.0,
9629                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9630         calibrator.getInitialBiasZAsAcceleration(bz2);
9631         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9632         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9633         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9634         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9635         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9636         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9637         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9638         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9639         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9640         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9641         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9642         final double[] bias1 = calibrator.getInitialBias();
9643         assertArrayEquals(bias1, new double[3], 0.0);
9644         final double[] bias2 = new double[3];
9645         calibrator.getInitialBias(bias2);
9646         assertArrayEquals(bias1, bias2, 0.0);
9647         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9648         assertEquals(b1, new Matrix(3, 1));
9649         final Matrix b2 = new Matrix(3, 1);
9650         calibrator.getInitialBiasAsMatrix(b2);
9651         assertEquals(b1, b2);
9652         final Matrix ma1 = calibrator.getInitialMa();
9653         assertEquals(ma1, new Matrix(3, 3));
9654         final Matrix ma2 = new Matrix(3, 3);
9655         calibrator.getInitialMa(ma2);
9656         assertEquals(ma1, ma2);
9657         assertNull(calibrator.getMeasurements());
9658         assertTrue(calibrator.isCommonAxisUsed());
9659         assertSame(calibrator.getListener(), this);
9660         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9661         assertFalse(calibrator.isReady());
9662         assertFalse(calibrator.isRunning());
9663         assertNull(calibrator.getEstimatedBiases());
9664         assertFalse(calibrator.getEstimatedBiases(null));
9665         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9666         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9667         assertNull(calibrator.getEstimatedBiasFx());
9668         assertNull(calibrator.getEstimatedBiasFy());
9669         assertNull(calibrator.getEstimatedBiasFz());
9670         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9671         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9672         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9673         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9674         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9675         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9676         assertNull(calibrator.getEstimatedMa());
9677         assertNull(calibrator.getEstimatedSx());
9678         assertNull(calibrator.getEstimatedSy());
9679         assertNull(calibrator.getEstimatedSz());
9680         assertNull(calibrator.getEstimatedMxy());
9681         assertNull(calibrator.getEstimatedMxz());
9682         assertNull(calibrator.getEstimatedMyx());
9683         assertNull(calibrator.getEstimatedMyz());
9684         assertNull(calibrator.getEstimatedMzx());
9685         assertNull(calibrator.getEstimatedMzy());
9686         assertNull(calibrator.getEstimatedCovariance());
9687         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9688         assertNotNull(calibrator.getGroundTruthGravityNorm());
9689         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9690         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9691         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9692                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9693         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9694         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9695         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9696 
9697         // Force IllegalArgumentException
9698         calibrator = null;
9699         try {
9700             calibrator = new KnownGravityNormAccelerometerCalibrator(
9701                     -gravityNorm, true, this);
9702             fail("IllegalArgumentException expected but not thrown");
9703         } catch (final IllegalArgumentException ignore) {
9704         }
9705         assertNull(calibrator);
9706     }
9707 
9708     @Test
9709     public void testConstructor85() throws WrongSizeException {
9710         final Collection<StandardDeviationBodyKinematics> measurements =
9711                 Collections.emptyList();
9712         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9713         final double latitude = Math.toRadians(
9714                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9715         final double longitude = Math.toRadians(
9716                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9717         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9718         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9719         final NEDVelocity nedVelocity = new NEDVelocity();
9720         final ECEFPosition ecefPosition = new ECEFPosition();
9721         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9722         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9723                 ecefPosition, ecefVelocity);
9724         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9725                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9726         final double gravityNorm = gravity.getNorm();
9727 
9728         KnownGravityNormAccelerometerCalibrator calibrator =
9729                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
9730                         measurements, true);
9731 
9732         // check default values
9733         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9734         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9735         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9736         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9737         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9738         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9739         final Acceleration bx2 = new Acceleration(0.0,
9740                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9741         calibrator.getInitialBiasXAsAcceleration(bx2);
9742         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9743         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9744         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9745         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9746         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9747         final Acceleration by2 = new Acceleration(0.0,
9748                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9749         calibrator.getInitialBiasYAsAcceleration(by2);
9750         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9751         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9752         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9753         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9754         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9755         final Acceleration bz2 = new Acceleration(0.0,
9756                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9757         calibrator.getInitialBiasZAsAcceleration(bz2);
9758         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9759         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9760         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9761         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9762         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9763         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9764         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9765         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9766         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9767         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9768         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9769         final double[] bias1 = calibrator.getInitialBias();
9770         assertArrayEquals(bias1, new double[3], 0.0);
9771         final double[] bias2 = new double[3];
9772         calibrator.getInitialBias(bias2);
9773         assertArrayEquals(bias1, bias2, 0.0);
9774         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9775         assertEquals(b1, new Matrix(3, 1));
9776         final Matrix b2 = new Matrix(3, 1);
9777         calibrator.getInitialBiasAsMatrix(b2);
9778         assertEquals(b1, b2);
9779         final Matrix ma1 = calibrator.getInitialMa();
9780         assertEquals(ma1, new Matrix(3, 3));
9781         final Matrix ma2 = new Matrix(3, 3);
9782         calibrator.getInitialMa(ma2);
9783         assertEquals(ma1, ma2);
9784         assertSame(calibrator.getMeasurements(), measurements);
9785         assertTrue(calibrator.isCommonAxisUsed());
9786         assertNull(calibrator.getListener());
9787         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9788         assertFalse(calibrator.isReady());
9789         assertFalse(calibrator.isRunning());
9790         assertNull(calibrator.getEstimatedBiases());
9791         assertFalse(calibrator.getEstimatedBiases(null));
9792         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9793         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9794         assertNull(calibrator.getEstimatedBiasFx());
9795         assertNull(calibrator.getEstimatedBiasFy());
9796         assertNull(calibrator.getEstimatedBiasFz());
9797         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9798         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9799         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9800         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9801         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9802         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9803         assertNull(calibrator.getEstimatedMa());
9804         assertNull(calibrator.getEstimatedSx());
9805         assertNull(calibrator.getEstimatedSy());
9806         assertNull(calibrator.getEstimatedSz());
9807         assertNull(calibrator.getEstimatedMxy());
9808         assertNull(calibrator.getEstimatedMxz());
9809         assertNull(calibrator.getEstimatedMyx());
9810         assertNull(calibrator.getEstimatedMyz());
9811         assertNull(calibrator.getEstimatedMzx());
9812         assertNull(calibrator.getEstimatedMzy());
9813         assertNull(calibrator.getEstimatedCovariance());
9814         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9815         assertNotNull(calibrator.getGroundTruthGravityNorm());
9816         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9817         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9818         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9819                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9820         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9821         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9822         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9823 
9824         // Force IllegalArgumentException
9825         calibrator = null;
9826         try {
9827             calibrator = new KnownGravityNormAccelerometerCalibrator(
9828                     -gravityNorm, measurements, true);
9829             fail("IllegalArgumentException expected but not thrown");
9830         } catch (final IllegalArgumentException ignore) {
9831         }
9832         assertNull(calibrator);
9833     }
9834 
9835     @Test
9836     public void testConstructor86() throws WrongSizeException {
9837         final Collection<StandardDeviationBodyKinematics> measurements =
9838                 Collections.emptyList();
9839         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9840         final double latitude = Math.toRadians(
9841                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9842         final double longitude = Math.toRadians(
9843                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9844         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9845         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9846         final NEDVelocity nedVelocity = new NEDVelocity();
9847         final ECEFPosition ecefPosition = new ECEFPosition();
9848         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9849         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9850                 ecefPosition, ecefVelocity);
9851         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9852                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9853         final double gravityNorm = gravity.getNorm();
9854 
9855         KnownGravityNormAccelerometerCalibrator calibrator =
9856                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
9857                         measurements, true, this);
9858 
9859         // check default values
9860         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9861         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9862         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9863         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9864         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9865         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9866         final Acceleration bx2 = new Acceleration(0.0,
9867                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9868         calibrator.getInitialBiasXAsAcceleration(bx2);
9869         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9870         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9871         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9872         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9873         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9874         final Acceleration by2 = new Acceleration(0.0,
9875                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9876         calibrator.getInitialBiasYAsAcceleration(by2);
9877         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9878         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9879         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9880         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9881         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9882         final Acceleration bz2 = new Acceleration(0.0,
9883                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9884         calibrator.getInitialBiasZAsAcceleration(bz2);
9885         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9886         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9887         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9888         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9889         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9890         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9891         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9892         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9893         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9894         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9895         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9896         final double[] bias1 = calibrator.getInitialBias();
9897         assertArrayEquals(bias1, new double[3], 0.0);
9898         final double[] bias2 = new double[3];
9899         calibrator.getInitialBias(bias2);
9900         assertArrayEquals(bias1, bias2, 0.0);
9901         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9902         assertEquals(b1, new Matrix(3, 1));
9903         final Matrix b2 = new Matrix(3, 1);
9904         calibrator.getInitialBiasAsMatrix(b2);
9905         assertEquals(b1, b2);
9906         final Matrix ma1 = calibrator.getInitialMa();
9907         assertEquals(ma1, new Matrix(3, 3));
9908         final Matrix ma2 = new Matrix(3, 3);
9909         calibrator.getInitialMa(ma2);
9910         assertEquals(ma1, ma2);
9911         assertSame(calibrator.getMeasurements(), measurements);
9912         assertTrue(calibrator.isCommonAxisUsed());
9913         assertSame(calibrator.getListener(), this);
9914         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9915         assertFalse(calibrator.isReady());
9916         assertFalse(calibrator.isRunning());
9917         assertNull(calibrator.getEstimatedBiases());
9918         assertFalse(calibrator.getEstimatedBiases(null));
9919         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9920         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9921         assertNull(calibrator.getEstimatedBiasFx());
9922         assertNull(calibrator.getEstimatedBiasFy());
9923         assertNull(calibrator.getEstimatedBiasFz());
9924         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9925         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9926         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9927         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9928         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9929         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9930         assertNull(calibrator.getEstimatedMa());
9931         assertNull(calibrator.getEstimatedSx());
9932         assertNull(calibrator.getEstimatedSy());
9933         assertNull(calibrator.getEstimatedSz());
9934         assertNull(calibrator.getEstimatedMxy());
9935         assertNull(calibrator.getEstimatedMxz());
9936         assertNull(calibrator.getEstimatedMyx());
9937         assertNull(calibrator.getEstimatedMyz());
9938         assertNull(calibrator.getEstimatedMzx());
9939         assertNull(calibrator.getEstimatedMzy());
9940         assertNull(calibrator.getEstimatedCovariance());
9941         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9942         assertNotNull(calibrator.getGroundTruthGravityNorm());
9943         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9944         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9945         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9946                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9947         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9948         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9949         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9950 
9951         // Force IllegalArgumentException
9952         calibrator = null;
9953         try {
9954             calibrator = new KnownGravityNormAccelerometerCalibrator(
9955                     -gravityNorm, measurements, true, this);
9956             fail("IllegalArgumentException expected but not thrown");
9957         } catch (final IllegalArgumentException ignore) {
9958         }
9959         assertNull(calibrator);
9960     }
9961 
9962     @Test
9963     public void testConstructor87() throws WrongSizeException {
9964         final Matrix ba = generateBa();
9965         final double biasX = ba.getElementAtIndex(0);
9966         final double biasY = ba.getElementAtIndex(1);
9967         final double biasZ = ba.getElementAtIndex(2);
9968 
9969         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9970         final double latitude = Math.toRadians(
9971                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9972         final double longitude = Math.toRadians(
9973                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9974         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9975         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9976         final NEDVelocity nedVelocity = new NEDVelocity();
9977         final ECEFPosition ecefPosition = new ECEFPosition();
9978         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9979         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9980                 ecefPosition, ecefVelocity);
9981         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9982                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9983         final double gravityNorm = gravity.getNorm();
9984 
9985         KnownGravityNormAccelerometerCalibrator calibrator =
9986                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
9987                         biasX, biasY, biasZ);
9988 
9989         // check default values
9990         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
9991         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
9992         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
9993         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9994         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9995         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9996         final Acceleration bx2 = new Acceleration(0.0,
9997                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9998         calibrator.getInitialBiasXAsAcceleration(bx2);
9999         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10000         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10001         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10002         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10003         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10004         final Acceleration by2 = new Acceleration(0.0,
10005                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10006         calibrator.getInitialBiasYAsAcceleration(by2);
10007         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10008         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10009         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10010         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10011         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10012         final Acceleration bz2 = new Acceleration(0.0,
10013                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10014         calibrator.getInitialBiasZAsAcceleration(bz2);
10015         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10016         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10017         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10018         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10019         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10020         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10021         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10022         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10023         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10024         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10025         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10026         final double[] bias1 = calibrator.getInitialBias();
10027         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10028         final double[] bias2 = new double[3];
10029         calibrator.getInitialBias(bias2);
10030         assertArrayEquals(bias1, bias2, 0.0);
10031         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10032         assertEquals(b1, ba);
10033         final Matrix b2 = new Matrix(3, 1);
10034         calibrator.getInitialBiasAsMatrix(b2);
10035         assertEquals(b1, b2);
10036         final Matrix ma1 = calibrator.getInitialMa();
10037         assertEquals(ma1, new Matrix(3, 3));
10038         final Matrix ma2 = new Matrix(3, 3);
10039         calibrator.getInitialMa(ma2);
10040         assertEquals(ma1, ma2);
10041         assertNull(calibrator.getMeasurements());
10042         assertFalse(calibrator.isCommonAxisUsed());
10043         assertNull(calibrator.getListener());
10044         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10045         assertFalse(calibrator.isReady());
10046         assertFalse(calibrator.isRunning());
10047         assertNull(calibrator.getEstimatedBiases());
10048         assertFalse(calibrator.getEstimatedBiases(null));
10049         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10050         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10051         assertNull(calibrator.getEstimatedBiasFx());
10052         assertNull(calibrator.getEstimatedBiasFy());
10053         assertNull(calibrator.getEstimatedBiasFz());
10054         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10055         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10056         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10057         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10058         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10059         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10060         assertNull(calibrator.getEstimatedMa());
10061         assertNull(calibrator.getEstimatedSx());
10062         assertNull(calibrator.getEstimatedSy());
10063         assertNull(calibrator.getEstimatedSz());
10064         assertNull(calibrator.getEstimatedMxy());
10065         assertNull(calibrator.getEstimatedMxz());
10066         assertNull(calibrator.getEstimatedMyx());
10067         assertNull(calibrator.getEstimatedMyz());
10068         assertNull(calibrator.getEstimatedMzx());
10069         assertNull(calibrator.getEstimatedMzy());
10070         assertNull(calibrator.getEstimatedCovariance());
10071         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10072         assertNotNull(calibrator.getGroundTruthGravityNorm());
10073         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10074         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10075         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10076                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10077         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10078         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10079         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10080 
10081         // Force IllegalArgumentException
10082         calibrator = null;
10083         try {
10084             calibrator = new KnownGravityNormAccelerometerCalibrator(
10085                     -gravityNorm, biasX, biasY, biasZ);
10086             fail("IllegalArgumentException expected but not thrown");
10087         } catch (final IllegalArgumentException ignore) {
10088         }
10089         assertNull(calibrator);
10090     }
10091 
10092     @Test
10093     public void testConstructor88() throws WrongSizeException {
10094         final Matrix ba = generateBa();
10095         final double biasX = ba.getElementAtIndex(0);
10096         final double biasY = ba.getElementAtIndex(1);
10097         final double biasZ = ba.getElementAtIndex(2);
10098 
10099         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10100         final double latitude = Math.toRadians(
10101                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10102         final double longitude = Math.toRadians(
10103                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10104         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10105         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10106         final NEDVelocity nedVelocity = new NEDVelocity();
10107         final ECEFPosition ecefPosition = new ECEFPosition();
10108         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10109         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10110                 ecefPosition, ecefVelocity);
10111         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10112                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10113         final double gravityNorm = gravity.getNorm();
10114 
10115         KnownGravityNormAccelerometerCalibrator calibrator =
10116                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
10117                         biasX, biasY, biasZ, this);
10118 
10119         // check default values
10120         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10121         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10122         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10123         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10124         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10125         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10126         final Acceleration bx2 = new Acceleration(0.0,
10127                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10128         calibrator.getInitialBiasXAsAcceleration(bx2);
10129         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10130         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10131         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10132         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10133         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10134         final Acceleration by2 = new Acceleration(0.0,
10135                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10136         calibrator.getInitialBiasYAsAcceleration(by2);
10137         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10138         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10139         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10140         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10141         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10142         final Acceleration bz2 = new Acceleration(0.0,
10143                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10144         calibrator.getInitialBiasZAsAcceleration(bz2);
10145         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10146         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10147         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10148         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10149         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10150         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10151         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10152         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10153         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10154         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10155         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10156         final double[] bias1 = calibrator.getInitialBias();
10157         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10158         final double[] bias2 = new double[3];
10159         calibrator.getInitialBias(bias2);
10160         assertArrayEquals(bias1, bias2, 0.0);
10161         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10162         assertEquals(b1, ba);
10163         final Matrix b2 = new Matrix(3, 1);
10164         calibrator.getInitialBiasAsMatrix(b2);
10165         assertEquals(b1, b2);
10166         final Matrix ma1 = calibrator.getInitialMa();
10167         assertEquals(ma1, new Matrix(3, 3));
10168         final Matrix ma2 = new Matrix(3, 3);
10169         calibrator.getInitialMa(ma2);
10170         assertEquals(ma1, ma2);
10171         assertNull(calibrator.getMeasurements());
10172         assertFalse(calibrator.isCommonAxisUsed());
10173         assertSame(calibrator.getListener(), this);
10174         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10175         assertFalse(calibrator.isReady());
10176         assertFalse(calibrator.isRunning());
10177         assertNull(calibrator.getEstimatedBiases());
10178         assertFalse(calibrator.getEstimatedBiases(null));
10179         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10180         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10181         assertNull(calibrator.getEstimatedBiasFx());
10182         assertNull(calibrator.getEstimatedBiasFy());
10183         assertNull(calibrator.getEstimatedBiasFz());
10184         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10185         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10186         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10187         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10188         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10189         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10190         assertNull(calibrator.getEstimatedMa());
10191         assertNull(calibrator.getEstimatedSx());
10192         assertNull(calibrator.getEstimatedSy());
10193         assertNull(calibrator.getEstimatedSz());
10194         assertNull(calibrator.getEstimatedMxy());
10195         assertNull(calibrator.getEstimatedMxz());
10196         assertNull(calibrator.getEstimatedMyx());
10197         assertNull(calibrator.getEstimatedMyz());
10198         assertNull(calibrator.getEstimatedMzx());
10199         assertNull(calibrator.getEstimatedMzy());
10200         assertNull(calibrator.getEstimatedCovariance());
10201         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10202         assertNotNull(calibrator.getGroundTruthGravityNorm());
10203         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10204         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10205         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10206                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10207         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10208         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10209         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10210 
10211         // Force IllegalArgumentException
10212         calibrator = null;
10213         try {
10214             calibrator = new KnownGravityNormAccelerometerCalibrator(
10215                     -gravityNorm, biasX, biasY, biasZ, this);
10216             fail("IllegalArgumentException expected but not thrown");
10217         } catch (final IllegalArgumentException ignore) {
10218         }
10219         assertNull(calibrator);
10220     }
10221 
10222     @Test
10223     public void testConstructor89() throws WrongSizeException {
10224         final Collection<StandardDeviationBodyKinematics> measurements =
10225                 Collections.emptyList();
10226 
10227         final Matrix ba = generateBa();
10228         final double biasX = ba.getElementAtIndex(0);
10229         final double biasY = ba.getElementAtIndex(1);
10230         final double biasZ = ba.getElementAtIndex(2);
10231 
10232         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10233         final double latitude = Math.toRadians(
10234                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10235         final double longitude = Math.toRadians(
10236                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10237         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10238         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10239         final NEDVelocity nedVelocity = new NEDVelocity();
10240         final ECEFPosition ecefPosition = new ECEFPosition();
10241         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10242         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10243                 ecefPosition, ecefVelocity);
10244         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10245                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10246         final double gravityNorm = gravity.getNorm();
10247 
10248         KnownGravityNormAccelerometerCalibrator calibrator =
10249                 new KnownGravityNormAccelerometerCalibrator(
10250                         gravityNorm, measurements,
10251                         biasX, biasY, biasZ);
10252 
10253         // check default values
10254         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10255         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10256         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10257         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10258         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10259         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10260         final Acceleration bx2 = new Acceleration(0.0,
10261                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10262         calibrator.getInitialBiasXAsAcceleration(bx2);
10263         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10264         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10265         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10266         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10267         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10268         final Acceleration by2 = new Acceleration(0.0,
10269                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10270         calibrator.getInitialBiasYAsAcceleration(by2);
10271         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10272         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10273         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10274         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10275         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10276         final Acceleration bz2 = new Acceleration(0.0,
10277                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10278         calibrator.getInitialBiasZAsAcceleration(bz2);
10279         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10280         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10281         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10282         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10283         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10284         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10285         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10286         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10287         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10288         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10289         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10290         final double[] bias1 = calibrator.getInitialBias();
10291         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10292         final double[] bias2 = new double[3];
10293         calibrator.getInitialBias(bias2);
10294         assertArrayEquals(bias1, bias2, 0.0);
10295         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10296         assertEquals(b1, ba);
10297         final Matrix b2 = new Matrix(3, 1);
10298         calibrator.getInitialBiasAsMatrix(b2);
10299         assertEquals(b1, b2);
10300         final Matrix ma1 = calibrator.getInitialMa();
10301         assertEquals(ma1, new Matrix(3, 3));
10302         final Matrix ma2 = new Matrix(3, 3);
10303         calibrator.getInitialMa(ma2);
10304         assertEquals(ma1, ma2);
10305         assertSame(calibrator.getMeasurements(), measurements);
10306         assertFalse(calibrator.isCommonAxisUsed());
10307         assertNull(calibrator.getListener());
10308         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10309         assertFalse(calibrator.isReady());
10310         assertFalse(calibrator.isRunning());
10311         assertNull(calibrator.getEstimatedBiases());
10312         assertFalse(calibrator.getEstimatedBiases(null));
10313         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10314         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10315         assertNull(calibrator.getEstimatedBiasFx());
10316         assertNull(calibrator.getEstimatedBiasFy());
10317         assertNull(calibrator.getEstimatedBiasFz());
10318         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10319         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10320         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10321         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10322         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10323         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10324         assertNull(calibrator.getEstimatedMa());
10325         assertNull(calibrator.getEstimatedSx());
10326         assertNull(calibrator.getEstimatedSy());
10327         assertNull(calibrator.getEstimatedSz());
10328         assertNull(calibrator.getEstimatedMxy());
10329         assertNull(calibrator.getEstimatedMxz());
10330         assertNull(calibrator.getEstimatedMyx());
10331         assertNull(calibrator.getEstimatedMyz());
10332         assertNull(calibrator.getEstimatedMzx());
10333         assertNull(calibrator.getEstimatedMzy());
10334         assertNull(calibrator.getEstimatedCovariance());
10335         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10336         assertNotNull(calibrator.getGroundTruthGravityNorm());
10337         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10338         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10339         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10340                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10341         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10342         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10343         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10344 
10345         // Force IllegalArgumentException
10346         calibrator = null;
10347         try {
10348             calibrator = new KnownGravityNormAccelerometerCalibrator(
10349                     -gravityNorm, measurements,
10350                     biasX, biasY, biasZ);
10351             fail("IllegalArgumentException expected but not thrown");
10352         } catch (final IllegalArgumentException ignore) {
10353         }
10354         assertNull(calibrator);
10355     }
10356 
10357     @Test
10358     public void testConstructor90() throws WrongSizeException {
10359         final Collection<StandardDeviationBodyKinematics> measurements =
10360                 Collections.emptyList();
10361 
10362         final Matrix ba = generateBa();
10363         final double biasX = ba.getElementAtIndex(0);
10364         final double biasY = ba.getElementAtIndex(1);
10365         final double biasZ = ba.getElementAtIndex(2);
10366 
10367         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10368         final double latitude = Math.toRadians(
10369                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10370         final double longitude = Math.toRadians(
10371                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10372         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10373         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10374         final NEDVelocity nedVelocity = new NEDVelocity();
10375         final ECEFPosition ecefPosition = new ECEFPosition();
10376         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10377         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10378                 ecefPosition, ecefVelocity);
10379         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10380                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10381         final double gravityNorm = gravity.getNorm();
10382 
10383         KnownGravityNormAccelerometerCalibrator calibrator =
10384                 new KnownGravityNormAccelerometerCalibrator(
10385                         gravityNorm, measurements,
10386                         biasX, biasY, biasZ, this);
10387 
10388         // check default values
10389         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10390         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10391         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10392         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10393         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10394         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10395         final Acceleration bx2 = new Acceleration(0.0,
10396                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10397         calibrator.getInitialBiasXAsAcceleration(bx2);
10398         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10399         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10400         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10401         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10402         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10403         final Acceleration by2 = new Acceleration(0.0,
10404                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10405         calibrator.getInitialBiasYAsAcceleration(by2);
10406         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10407         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10408         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10409         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10410         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10411         final Acceleration bz2 = new Acceleration(0.0,
10412                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10413         calibrator.getInitialBiasZAsAcceleration(bz2);
10414         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10415         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10416         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10417         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10418         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10419         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10420         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10421         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10422         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10423         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10424         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10425         final double[] bias1 = calibrator.getInitialBias();
10426         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10427         final double[] bias2 = new double[3];
10428         calibrator.getInitialBias(bias2);
10429         assertArrayEquals(bias1, bias2, 0.0);
10430         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10431         assertEquals(b1, ba);
10432         final Matrix b2 = new Matrix(3, 1);
10433         calibrator.getInitialBiasAsMatrix(b2);
10434         assertEquals(b1, b2);
10435         final Matrix ma1 = calibrator.getInitialMa();
10436         assertEquals(ma1, new Matrix(3, 3));
10437         final Matrix ma2 = new Matrix(3, 3);
10438         calibrator.getInitialMa(ma2);
10439         assertEquals(ma1, ma2);
10440         assertSame(calibrator.getMeasurements(), measurements);
10441         assertFalse(calibrator.isCommonAxisUsed());
10442         assertSame(calibrator.getListener(), this);
10443         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10444         assertFalse(calibrator.isReady());
10445         assertFalse(calibrator.isRunning());
10446         assertNull(calibrator.getEstimatedBiases());
10447         assertFalse(calibrator.getEstimatedBiases(null));
10448         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10449         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10450         assertNull(calibrator.getEstimatedBiasFx());
10451         assertNull(calibrator.getEstimatedBiasFy());
10452         assertNull(calibrator.getEstimatedBiasFz());
10453         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10454         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10455         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10456         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10457         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10458         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10459         assertNull(calibrator.getEstimatedMa());
10460         assertNull(calibrator.getEstimatedSx());
10461         assertNull(calibrator.getEstimatedSy());
10462         assertNull(calibrator.getEstimatedSz());
10463         assertNull(calibrator.getEstimatedMxy());
10464         assertNull(calibrator.getEstimatedMxz());
10465         assertNull(calibrator.getEstimatedMyx());
10466         assertNull(calibrator.getEstimatedMyz());
10467         assertNull(calibrator.getEstimatedMzx());
10468         assertNull(calibrator.getEstimatedMzy());
10469         assertNull(calibrator.getEstimatedCovariance());
10470         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10471         assertNotNull(calibrator.getGroundTruthGravityNorm());
10472         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10473         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10474         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10475                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10476         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10477         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10478         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10479 
10480         // Force IllegalArgumentException
10481         calibrator = null;
10482         try {
10483             calibrator = new KnownGravityNormAccelerometerCalibrator(
10484                     -gravityNorm, measurements,
10485                     biasX, biasY, biasZ, this);
10486             fail("IllegalArgumentException expected but not thrown");
10487         } catch (final IllegalArgumentException ignore) {
10488         }
10489         assertNull(calibrator);
10490     }
10491 
10492     @Test
10493     public void testConstructor91() throws WrongSizeException {
10494         final Matrix ba = generateBa();
10495         final double biasX = ba.getElementAtIndex(0);
10496         final double biasY = ba.getElementAtIndex(1);
10497         final double biasZ = ba.getElementAtIndex(2);
10498 
10499         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10500         final double latitude = Math.toRadians(
10501                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10502         final double longitude = Math.toRadians(
10503                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10504         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10505         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10506         final NEDVelocity nedVelocity = new NEDVelocity();
10507         final ECEFPosition ecefPosition = new ECEFPosition();
10508         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10509         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10510                 ecefPosition, ecefVelocity);
10511         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10512                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10513         final double gravityNorm = gravity.getNorm();
10514 
10515         KnownGravityNormAccelerometerCalibrator calibrator =
10516                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
10517                         true, biasX, biasY, biasZ);
10518 
10519         // check default values
10520         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10521         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10522         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10523         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10524         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10525         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10526         final Acceleration bx2 = new Acceleration(0.0,
10527                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10528         calibrator.getInitialBiasXAsAcceleration(bx2);
10529         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10530         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10531         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10532         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10533         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10534         final Acceleration by2 = new Acceleration(0.0,
10535                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10536         calibrator.getInitialBiasYAsAcceleration(by2);
10537         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10538         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10539         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10540         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10541         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10542         final Acceleration bz2 = new Acceleration(0.0,
10543                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10544         calibrator.getInitialBiasZAsAcceleration(bz2);
10545         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10546         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10547         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10548         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10549         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10550         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10551         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10552         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10553         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10554         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10555         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10556         final double[] bias1 = calibrator.getInitialBias();
10557         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10558         final double[] bias2 = new double[3];
10559         calibrator.getInitialBias(bias2);
10560         assertArrayEquals(bias1, bias2, 0.0);
10561         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10562         assertEquals(b1, ba);
10563         final Matrix b2 = new Matrix(3, 1);
10564         calibrator.getInitialBiasAsMatrix(b2);
10565         assertEquals(b1, b2);
10566         final Matrix ma1 = calibrator.getInitialMa();
10567         assertEquals(ma1, new Matrix(3, 3));
10568         final Matrix ma2 = new Matrix(3, 3);
10569         calibrator.getInitialMa(ma2);
10570         assertEquals(ma1, ma2);
10571         assertNull(calibrator.getMeasurements());
10572         assertTrue(calibrator.isCommonAxisUsed());
10573         assertNull(calibrator.getListener());
10574         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10575         assertFalse(calibrator.isReady());
10576         assertFalse(calibrator.isRunning());
10577         assertNull(calibrator.getEstimatedBiases());
10578         assertFalse(calibrator.getEstimatedBiases(null));
10579         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10580         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10581         assertNull(calibrator.getEstimatedBiasFx());
10582         assertNull(calibrator.getEstimatedBiasFy());
10583         assertNull(calibrator.getEstimatedBiasFz());
10584         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10585         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10586         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10587         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10588         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10589         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10590         assertNull(calibrator.getEstimatedMa());
10591         assertNull(calibrator.getEstimatedSx());
10592         assertNull(calibrator.getEstimatedSy());
10593         assertNull(calibrator.getEstimatedSz());
10594         assertNull(calibrator.getEstimatedMxy());
10595         assertNull(calibrator.getEstimatedMxz());
10596         assertNull(calibrator.getEstimatedMyx());
10597         assertNull(calibrator.getEstimatedMyz());
10598         assertNull(calibrator.getEstimatedMzx());
10599         assertNull(calibrator.getEstimatedMzy());
10600         assertNull(calibrator.getEstimatedCovariance());
10601         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10602         assertNotNull(calibrator.getGroundTruthGravityNorm());
10603         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10604         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10605         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10606                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10607         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10608         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10609         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10610 
10611         // Force IllegalArgumentException
10612         calibrator = null;
10613         try {
10614             calibrator = new KnownGravityNormAccelerometerCalibrator(
10615                     -gravityNorm, true, biasX, biasY, biasZ);
10616             fail("IllegalArgumentException expected but not thrown");
10617         } catch (final IllegalArgumentException ignore) {
10618         }
10619         assertNull(calibrator);
10620     }
10621 
10622     @Test
10623     public void testConstructor92() throws WrongSizeException {
10624         final Matrix ba = generateBa();
10625         final double biasX = ba.getElementAtIndex(0);
10626         final double biasY = ba.getElementAtIndex(1);
10627         final double biasZ = ba.getElementAtIndex(2);
10628 
10629         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10630         final double latitude = Math.toRadians(
10631                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10632         final double longitude = Math.toRadians(
10633                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10634         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10635         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10636         final NEDVelocity nedVelocity = new NEDVelocity();
10637         final ECEFPosition ecefPosition = new ECEFPosition();
10638         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10639         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10640                 ecefPosition, ecefVelocity);
10641         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10642                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10643         final double gravityNorm = gravity.getNorm();
10644 
10645         KnownGravityNormAccelerometerCalibrator calibrator =
10646                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
10647                         true, biasX, biasY, biasZ,
10648                         this);
10649 
10650         // check default values
10651         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10652         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10653         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10654         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10655         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10656         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10657         final Acceleration bx2 = new Acceleration(0.0,
10658                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10659         calibrator.getInitialBiasXAsAcceleration(bx2);
10660         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10661         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10662         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10663         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10664         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10665         final Acceleration by2 = new Acceleration(0.0,
10666                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10667         calibrator.getInitialBiasYAsAcceleration(by2);
10668         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10669         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10670         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10671         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10672         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10673         final Acceleration bz2 = new Acceleration(0.0,
10674                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10675         calibrator.getInitialBiasZAsAcceleration(bz2);
10676         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10677         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10678         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10679         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10680         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10681         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10682         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10683         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10684         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10685         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10686         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10687         final double[] bias1 = calibrator.getInitialBias();
10688         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10689         final double[] bias2 = new double[3];
10690         calibrator.getInitialBias(bias2);
10691         assertArrayEquals(bias1, bias2, 0.0);
10692         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10693         assertEquals(b1, ba);
10694         final Matrix b2 = new Matrix(3, 1);
10695         calibrator.getInitialBiasAsMatrix(b2);
10696         assertEquals(b1, b2);
10697         final Matrix ma1 = calibrator.getInitialMa();
10698         assertEquals(ma1, new Matrix(3, 3));
10699         final Matrix ma2 = new Matrix(3, 3);
10700         calibrator.getInitialMa(ma2);
10701         assertEquals(ma1, ma2);
10702         assertNull(calibrator.getMeasurements());
10703         assertTrue(calibrator.isCommonAxisUsed());
10704         assertSame(calibrator.getListener(), this);
10705         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10706         assertFalse(calibrator.isReady());
10707         assertFalse(calibrator.isRunning());
10708         assertNull(calibrator.getEstimatedBiases());
10709         assertFalse(calibrator.getEstimatedBiases(null));
10710         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10711         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10712         assertNull(calibrator.getEstimatedBiasFx());
10713         assertNull(calibrator.getEstimatedBiasFy());
10714         assertNull(calibrator.getEstimatedBiasFz());
10715         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10716         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10717         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10718         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10719         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10720         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10721         assertNull(calibrator.getEstimatedMa());
10722         assertNull(calibrator.getEstimatedSx());
10723         assertNull(calibrator.getEstimatedSy());
10724         assertNull(calibrator.getEstimatedSz());
10725         assertNull(calibrator.getEstimatedMxy());
10726         assertNull(calibrator.getEstimatedMxz());
10727         assertNull(calibrator.getEstimatedMyx());
10728         assertNull(calibrator.getEstimatedMyz());
10729         assertNull(calibrator.getEstimatedMzx());
10730         assertNull(calibrator.getEstimatedMzy());
10731         assertNull(calibrator.getEstimatedCovariance());
10732         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10733         assertNotNull(calibrator.getGroundTruthGravityNorm());
10734         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10735         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10736         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10737                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10738         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10739         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10740         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10741 
10742         // Force IllegalArgumentException
10743         calibrator = null;
10744         try {
10745             calibrator = new KnownGravityNormAccelerometerCalibrator(
10746                     -gravityNorm, true, biasX, biasY, biasZ,
10747                     this);
10748             fail("IllegalArgumentException expected but not thrown");
10749         } catch (final IllegalArgumentException ignore) {
10750         }
10751         assertNull(calibrator);
10752     }
10753 
10754     @Test
10755     public void testConstructor93() throws WrongSizeException {
10756         final Collection<StandardDeviationBodyKinematics> measurements =
10757                 Collections.emptyList();
10758 
10759         final Matrix ba = generateBa();
10760         final double biasX = ba.getElementAtIndex(0);
10761         final double biasY = ba.getElementAtIndex(1);
10762         final double biasZ = ba.getElementAtIndex(2);
10763 
10764         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10765         final double latitude = Math.toRadians(
10766                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10767         final double longitude = Math.toRadians(
10768                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10769         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10770         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10771         final NEDVelocity nedVelocity = new NEDVelocity();
10772         final ECEFPosition ecefPosition = new ECEFPosition();
10773         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10774         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10775                 ecefPosition, ecefVelocity);
10776         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10777                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10778         final double gravityNorm = gravity.getNorm();
10779 
10780         KnownGravityNormAccelerometerCalibrator calibrator =
10781                 new KnownGravityNormAccelerometerCalibrator(
10782                         gravityNorm, measurements,
10783                         true, biasX, biasY, biasZ);
10784 
10785         // check default values
10786         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10787         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10788         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10789         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10790         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10791         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10792         final Acceleration bx2 = new Acceleration(0.0,
10793                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10794         calibrator.getInitialBiasXAsAcceleration(bx2);
10795         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10796         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10797         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10798         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10799         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10800         final Acceleration by2 = new Acceleration(0.0,
10801                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10802         calibrator.getInitialBiasYAsAcceleration(by2);
10803         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10804         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10805         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10806         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10807         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10808         final Acceleration bz2 = new Acceleration(0.0,
10809                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10810         calibrator.getInitialBiasZAsAcceleration(bz2);
10811         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10812         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10813         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10814         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10815         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10816         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10817         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10818         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10819         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10820         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10821         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10822         final double[] bias1 = calibrator.getInitialBias();
10823         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10824         final double[] bias2 = new double[3];
10825         calibrator.getInitialBias(bias2);
10826         assertArrayEquals(bias1, bias2, 0.0);
10827         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10828         assertEquals(b1, ba);
10829         final Matrix b2 = new Matrix(3, 1);
10830         calibrator.getInitialBiasAsMatrix(b2);
10831         assertEquals(b1, b2);
10832         final Matrix ma1 = calibrator.getInitialMa();
10833         assertEquals(ma1, new Matrix(3, 3));
10834         final Matrix ma2 = new Matrix(3, 3);
10835         calibrator.getInitialMa(ma2);
10836         assertEquals(ma1, ma2);
10837         assertSame(calibrator.getMeasurements(), measurements);
10838         assertTrue(calibrator.isCommonAxisUsed());
10839         assertNull(calibrator.getListener());
10840         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10841         assertFalse(calibrator.isReady());
10842         assertFalse(calibrator.isRunning());
10843         assertNull(calibrator.getEstimatedBiases());
10844         assertFalse(calibrator.getEstimatedBiases(null));
10845         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10846         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10847         assertNull(calibrator.getEstimatedBiasFx());
10848         assertNull(calibrator.getEstimatedBiasFy());
10849         assertNull(calibrator.getEstimatedBiasFz());
10850         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10851         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10852         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10853         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10854         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10855         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10856         assertNull(calibrator.getEstimatedMa());
10857         assertNull(calibrator.getEstimatedSx());
10858         assertNull(calibrator.getEstimatedSy());
10859         assertNull(calibrator.getEstimatedSz());
10860         assertNull(calibrator.getEstimatedMxy());
10861         assertNull(calibrator.getEstimatedMxz());
10862         assertNull(calibrator.getEstimatedMyx());
10863         assertNull(calibrator.getEstimatedMyz());
10864         assertNull(calibrator.getEstimatedMzx());
10865         assertNull(calibrator.getEstimatedMzy());
10866         assertNull(calibrator.getEstimatedCovariance());
10867         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10868         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10869         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10870         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10871                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10872         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10873         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10874         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10875 
10876         // Force IllegalArgumentException
10877         calibrator = null;
10878         try {
10879             calibrator = new KnownGravityNormAccelerometerCalibrator(
10880                     -gravityNorm, measurements,
10881                     true, biasX, biasY, biasZ);
10882             fail("IllegalArgumentException expected but not thrown");
10883         } catch (final IllegalArgumentException ignore) {
10884         }
10885         assertNull(calibrator);
10886     }
10887 
10888     @Test
10889     public void testConstructor94() throws WrongSizeException {
10890         final Collection<StandardDeviationBodyKinematics> measurements =
10891                 Collections.emptyList();
10892 
10893         final Matrix ba = generateBa();
10894         final double biasX = ba.getElementAtIndex(0);
10895         final double biasY = ba.getElementAtIndex(1);
10896         final double biasZ = ba.getElementAtIndex(2);
10897 
10898         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10899         final double latitude = Math.toRadians(
10900                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10901         final double longitude = Math.toRadians(
10902                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10903         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10904         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10905         final NEDVelocity nedVelocity = new NEDVelocity();
10906         final ECEFPosition ecefPosition = new ECEFPosition();
10907         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10908         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10909                 ecefPosition, ecefVelocity);
10910         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10911                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10912         final double gravityNorm = gravity.getNorm();
10913 
10914         KnownGravityNormAccelerometerCalibrator calibrator =
10915                 new KnownGravityNormAccelerometerCalibrator(
10916                         gravityNorm, measurements,
10917                         true, biasX, biasY, biasZ, this);
10918 
10919         // check default values
10920         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10921         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10922         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10923         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10924         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10925         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10926         final Acceleration bx2 = new Acceleration(0.0,
10927                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10928         calibrator.getInitialBiasXAsAcceleration(bx2);
10929         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10930         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10931         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10932         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10933         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10934         final Acceleration by2 = new Acceleration(0.0,
10935                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10936         calibrator.getInitialBiasYAsAcceleration(by2);
10937         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10938         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10939         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10940         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10941         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10942         final Acceleration bz2 = new Acceleration(0.0,
10943                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10944         calibrator.getInitialBiasZAsAcceleration(bz2);
10945         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10946         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10947         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10948         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10949         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10950         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10951         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10952         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10953         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10954         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10955         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10956         final double[] bias1 = calibrator.getInitialBias();
10957         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10958         final double[] bias2 = new double[3];
10959         calibrator.getInitialBias(bias2);
10960         assertArrayEquals(bias1, bias2, 0.0);
10961         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10962         assertEquals(b1, ba);
10963         final Matrix b2 = new Matrix(3, 1);
10964         calibrator.getInitialBiasAsMatrix(b2);
10965         assertEquals(b1, b2);
10966         final Matrix ma1 = calibrator.getInitialMa();
10967         assertEquals(ma1, new Matrix(3, 3));
10968         final Matrix ma2 = new Matrix(3, 3);
10969         calibrator.getInitialMa(ma2);
10970         assertEquals(ma1, ma2);
10971         assertSame(calibrator.getMeasurements(), measurements);
10972         assertTrue(calibrator.isCommonAxisUsed());
10973         assertSame(calibrator.getListener(), this);
10974         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10975         assertFalse(calibrator.isReady());
10976         assertFalse(calibrator.isRunning());
10977         assertNull(calibrator.getEstimatedBiases());
10978         assertFalse(calibrator.getEstimatedBiases(null));
10979         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10980         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10981         assertNull(calibrator.getEstimatedBiasFx());
10982         assertNull(calibrator.getEstimatedBiasFy());
10983         assertNull(calibrator.getEstimatedBiasFz());
10984         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10985         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10986         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10987         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10988         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10989         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10990         assertNull(calibrator.getEstimatedMa());
10991         assertNull(calibrator.getEstimatedSx());
10992         assertNull(calibrator.getEstimatedSy());
10993         assertNull(calibrator.getEstimatedSz());
10994         assertNull(calibrator.getEstimatedMxy());
10995         assertNull(calibrator.getEstimatedMxz());
10996         assertNull(calibrator.getEstimatedMyx());
10997         assertNull(calibrator.getEstimatedMyz());
10998         assertNull(calibrator.getEstimatedMzx());
10999         assertNull(calibrator.getEstimatedMzy());
11000         assertNull(calibrator.getEstimatedCovariance());
11001         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11002         assertNotNull(calibrator.getGroundTruthGravityNorm());
11003         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11004         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11005         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11006                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11007         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11008         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11009         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11010 
11011         // Force IllegalArgumentException
11012         calibrator = null;
11013         try {
11014             calibrator = new KnownGravityNormAccelerometerCalibrator(
11015                     -gravityNorm, measurements,
11016                     true, biasX, biasY, biasZ, this);
11017             fail("IllegalArgumentException expected but not thrown");
11018         } catch (final IllegalArgumentException ignore) {
11019         }
11020         assertNull(calibrator);
11021     }
11022 
11023     @Test
11024     public void testConstructor95() throws WrongSizeException {
11025         final Matrix ba = generateBa();
11026         final double biasX = ba.getElementAtIndex(0);
11027         final double biasY = ba.getElementAtIndex(1);
11028         final double biasZ = ba.getElementAtIndex(2);
11029 
11030         final Acceleration bx = new Acceleration(biasX,
11031                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11032         final Acceleration by = new Acceleration(biasY,
11033                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11034         final Acceleration bz = new Acceleration(biasZ,
11035                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11036 
11037         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11038         final double latitude = Math.toRadians(
11039                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11040         final double longitude = Math.toRadians(
11041                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11042         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11043         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11044         final NEDVelocity nedVelocity = new NEDVelocity();
11045         final ECEFPosition ecefPosition = new ECEFPosition();
11046         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11047         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11048                 ecefPosition, ecefVelocity);
11049         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11050                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11051         final double gravityNorm = gravity.getNorm();
11052 
11053         KnownGravityNormAccelerometerCalibrator calibrator =
11054                 new KnownGravityNormAccelerometerCalibrator(
11055                         gravityNorm, bx, by, bz);
11056 
11057         // check default values
11058         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11059         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11060         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11061         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11062         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11063         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11064         final Acceleration bx2 = new Acceleration(0.0,
11065                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11066         calibrator.getInitialBiasXAsAcceleration(bx2);
11067         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11068         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11069         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11070         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11071         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11072         final Acceleration by2 = new Acceleration(0.0,
11073                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11074         calibrator.getInitialBiasYAsAcceleration(by2);
11075         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11076         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11077         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11078         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11079         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11080         final Acceleration bz2 = new Acceleration(0.0,
11081                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11082         calibrator.getInitialBiasZAsAcceleration(bz2);
11083         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11084         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11085         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11086         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11087         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11088         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11089         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11090         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11091         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11092         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11093         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11094         final double[] bias1 = calibrator.getInitialBias();
11095         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11096         final double[] bias2 = new double[3];
11097         calibrator.getInitialBias(bias2);
11098         assertArrayEquals(bias1, bias2, 0.0);
11099         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11100         assertEquals(b1, ba);
11101         final Matrix b2 = new Matrix(3, 1);
11102         calibrator.getInitialBiasAsMatrix(b2);
11103         assertEquals(b1, b2);
11104         final Matrix ma1 = calibrator.getInitialMa();
11105         assertEquals(ma1, new Matrix(3, 3));
11106         final Matrix ma2 = new Matrix(3, 3);
11107         calibrator.getInitialMa(ma2);
11108         assertEquals(ma1, ma2);
11109         assertNull(calibrator.getMeasurements());
11110         assertFalse(calibrator.isCommonAxisUsed());
11111         assertNull(calibrator.getListener());
11112         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11113         assertFalse(calibrator.isReady());
11114         assertFalse(calibrator.isRunning());
11115         assertNull(calibrator.getEstimatedBiases());
11116         assertFalse(calibrator.getEstimatedBiases(null));
11117         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11118         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11119         assertNull(calibrator.getEstimatedBiasFx());
11120         assertNull(calibrator.getEstimatedBiasFy());
11121         assertNull(calibrator.getEstimatedBiasFz());
11122         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11123         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11124         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11125         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11126         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11127         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11128         assertNull(calibrator.getEstimatedMa());
11129         assertNull(calibrator.getEstimatedSx());
11130         assertNull(calibrator.getEstimatedSy());
11131         assertNull(calibrator.getEstimatedSz());
11132         assertNull(calibrator.getEstimatedMxy());
11133         assertNull(calibrator.getEstimatedMxz());
11134         assertNull(calibrator.getEstimatedMyx());
11135         assertNull(calibrator.getEstimatedMyz());
11136         assertNull(calibrator.getEstimatedMzx());
11137         assertNull(calibrator.getEstimatedMzy());
11138         assertNull(calibrator.getEstimatedCovariance());
11139         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11140         assertNotNull(calibrator.getGroundTruthGravityNorm());
11141         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11142         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11143         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11144                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11145         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11146         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11147         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11148 
11149         // Force IllegalArgumentException
11150         calibrator = null;
11151         try {
11152             calibrator = new KnownGravityNormAccelerometerCalibrator(
11153                     -gravityNorm, bx, by, bz);
11154             fail("IllegalArgumentException expected but not thrown");
11155         } catch (final IllegalArgumentException ignore) {
11156         }
11157         assertNull(calibrator);
11158     }
11159 
11160     @Test
11161     public void testConstructor96() throws WrongSizeException {
11162         final Matrix ba = generateBa();
11163         final double biasX = ba.getElementAtIndex(0);
11164         final double biasY = ba.getElementAtIndex(1);
11165         final double biasZ = ba.getElementAtIndex(2);
11166 
11167         final Acceleration bx = new Acceleration(biasX,
11168                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11169         final Acceleration by = new Acceleration(biasY,
11170                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11171         final Acceleration bz = new Acceleration(biasZ,
11172                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11173 
11174         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11175         final double latitude = Math.toRadians(
11176                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11177         final double longitude = Math.toRadians(
11178                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11179         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11180         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11181         final NEDVelocity nedVelocity = new NEDVelocity();
11182         final ECEFPosition ecefPosition = new ECEFPosition();
11183         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11184         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11185                 ecefPosition, ecefVelocity);
11186         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11187                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11188         final double gravityNorm = gravity.getNorm();
11189 
11190         KnownGravityNormAccelerometerCalibrator calibrator =
11191                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
11192                         bx, by, bz, this);
11193 
11194         // check default values
11195         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11196         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11197         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11198         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11199         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11200         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11201         final Acceleration bx2 = new Acceleration(0.0,
11202                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11203         calibrator.getInitialBiasXAsAcceleration(bx2);
11204         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11205         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11206         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11207         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11208         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11209         final Acceleration by2 = new Acceleration(0.0,
11210                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11211         calibrator.getInitialBiasYAsAcceleration(by2);
11212         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11213         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11214         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11215         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11216         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11217         final Acceleration bz2 = new Acceleration(0.0,
11218                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11219         calibrator.getInitialBiasZAsAcceleration(bz2);
11220         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11221         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11222         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11223         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11224         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11225         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11226         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11227         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11228         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11229         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11230         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11231         final double[] bias1 = calibrator.getInitialBias();
11232         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11233         final double[] bias2 = new double[3];
11234         calibrator.getInitialBias(bias2);
11235         assertArrayEquals(bias1, bias2, 0.0);
11236         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11237         assertEquals(b1, ba);
11238         final Matrix b2 = new Matrix(3, 1);
11239         calibrator.getInitialBiasAsMatrix(b2);
11240         assertEquals(b1, b2);
11241         final Matrix ma1 = calibrator.getInitialMa();
11242         assertEquals(ma1, new Matrix(3, 3));
11243         final Matrix ma2 = new Matrix(3, 3);
11244         calibrator.getInitialMa(ma2);
11245         assertEquals(ma1, ma2);
11246         assertNull(calibrator.getMeasurements());
11247         assertFalse(calibrator.isCommonAxisUsed());
11248         assertSame(calibrator.getListener(), this);
11249         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11250         assertFalse(calibrator.isReady());
11251         assertFalse(calibrator.isRunning());
11252         assertNull(calibrator.getEstimatedBiases());
11253         assertFalse(calibrator.getEstimatedBiases(null));
11254         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11255         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11256         assertNull(calibrator.getEstimatedBiasFx());
11257         assertNull(calibrator.getEstimatedBiasFy());
11258         assertNull(calibrator.getEstimatedBiasFz());
11259         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11260         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11261         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11262         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11263         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11264         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11265         assertNull(calibrator.getEstimatedMa());
11266         assertNull(calibrator.getEstimatedSx());
11267         assertNull(calibrator.getEstimatedSy());
11268         assertNull(calibrator.getEstimatedSz());
11269         assertNull(calibrator.getEstimatedMxy());
11270         assertNull(calibrator.getEstimatedMxz());
11271         assertNull(calibrator.getEstimatedMyx());
11272         assertNull(calibrator.getEstimatedMyz());
11273         assertNull(calibrator.getEstimatedMzx());
11274         assertNull(calibrator.getEstimatedMzy());
11275         assertNull(calibrator.getEstimatedCovariance());
11276         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11277         assertNotNull(calibrator.getGroundTruthGravityNorm());
11278         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11279         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11280         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11281                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11282         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11283         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11284         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11285 
11286         // Force IllegalArgumentException
11287         calibrator = null;
11288         try {
11289             calibrator =
11290                     new KnownGravityNormAccelerometerCalibrator(
11291                             -gravityNorm, bx, by, bz, this);
11292             fail("IllegalArgumentException but not thrown");
11293         } catch (final IllegalArgumentException ignore) {
11294         }
11295         assertNull(calibrator);
11296     }
11297 
11298     @Test
11299     public void testConstructor97() throws WrongSizeException {
11300         final Collection<StandardDeviationBodyKinematics> measurements =
11301                 Collections.emptyList();
11302 
11303         final Matrix ba = generateBa();
11304         final double biasX = ba.getElementAtIndex(0);
11305         final double biasY = ba.getElementAtIndex(1);
11306         final double biasZ = ba.getElementAtIndex(2);
11307 
11308         final Acceleration bx = new Acceleration(biasX,
11309                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11310         final Acceleration by = new Acceleration(biasY,
11311                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11312         final Acceleration bz = new Acceleration(biasZ,
11313                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11314 
11315         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11316         final double latitude = Math.toRadians(
11317                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11318         final double longitude = Math.toRadians(
11319                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11320         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11321         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11322         final NEDVelocity nedVelocity = new NEDVelocity();
11323         final ECEFPosition ecefPosition = new ECEFPosition();
11324         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11325         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11326                 ecefPosition, ecefVelocity);
11327         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11328                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11329         final double gravityNorm = gravity.getNorm();
11330 
11331         KnownGravityNormAccelerometerCalibrator calibrator =
11332                 new KnownGravityNormAccelerometerCalibrator(
11333                         gravityNorm, measurements, bx, by, bz);
11334 
11335         // check default values
11336         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11337         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11338         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11339         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11340         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11341         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11342         final Acceleration bx2 = new Acceleration(0.0,
11343                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11344         calibrator.getInitialBiasXAsAcceleration(bx2);
11345         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11346         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11347         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11348         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11349         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11350         final Acceleration by2 = new Acceleration(0.0,
11351                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11352         calibrator.getInitialBiasYAsAcceleration(by2);
11353         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11354         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11355         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11356         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11357         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11358         final Acceleration bz2 = new Acceleration(0.0,
11359                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11360         calibrator.getInitialBiasZAsAcceleration(bz2);
11361         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11362         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11363         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11364         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11365         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11366         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11367         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11368         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11369         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11370         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11371         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11372         final double[] bias1 = calibrator.getInitialBias();
11373         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11374         final double[] bias2 = new double[3];
11375         calibrator.getInitialBias(bias2);
11376         assertArrayEquals(bias1, bias2, 0.0);
11377         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11378         assertEquals(b1, ba);
11379         final Matrix b2 = new Matrix(3, 1);
11380         calibrator.getInitialBiasAsMatrix(b2);
11381         assertEquals(b1, b2);
11382         final Matrix ma1 = calibrator.getInitialMa();
11383         assertEquals(ma1, new Matrix(3, 3));
11384         final Matrix ma2 = new Matrix(3, 3);
11385         calibrator.getInitialMa(ma2);
11386         assertEquals(ma1, ma2);
11387         assertSame(calibrator.getMeasurements(), measurements);
11388         assertFalse(calibrator.isCommonAxisUsed());
11389         assertNull(calibrator.getListener());
11390         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11391         assertFalse(calibrator.isReady());
11392         assertFalse(calibrator.isRunning());
11393         assertNull(calibrator.getEstimatedBiases());
11394         assertFalse(calibrator.getEstimatedBiases(null));
11395         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11396         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11397         assertNull(calibrator.getEstimatedBiasFx());
11398         assertNull(calibrator.getEstimatedBiasFy());
11399         assertNull(calibrator.getEstimatedBiasFz());
11400         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11401         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11402         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11403         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11404         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11405         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11406         assertNull(calibrator.getEstimatedMa());
11407         assertNull(calibrator.getEstimatedSx());
11408         assertNull(calibrator.getEstimatedSy());
11409         assertNull(calibrator.getEstimatedSz());
11410         assertNull(calibrator.getEstimatedMxy());
11411         assertNull(calibrator.getEstimatedMxz());
11412         assertNull(calibrator.getEstimatedMyx());
11413         assertNull(calibrator.getEstimatedMyz());
11414         assertNull(calibrator.getEstimatedMzx());
11415         assertNull(calibrator.getEstimatedMzy());
11416         assertNull(calibrator.getEstimatedCovariance());
11417         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11418         assertNotNull(calibrator.getGroundTruthGravityNorm());
11419         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11420         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11421         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11422                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11423         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11424         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11425         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11426 
11427         // Force IllegalArgumentException
11428         calibrator = null;
11429         try {
11430             calibrator = new KnownGravityNormAccelerometerCalibrator(
11431                     -gravityNorm, measurements, bx, by, bz);
11432             fail("IllegalArgumentException expected but not thrown");
11433         } catch (final IllegalArgumentException ignore) {
11434         }
11435         assertNull(calibrator);
11436     }
11437 
11438     @Test
11439     public void testConstructor98() throws WrongSizeException {
11440         final Collection<StandardDeviationBodyKinematics> measurements =
11441                 Collections.emptyList();
11442 
11443         final Matrix ba = generateBa();
11444         final double biasX = ba.getElementAtIndex(0);
11445         final double biasY = ba.getElementAtIndex(1);
11446         final double biasZ = ba.getElementAtIndex(2);
11447 
11448         final Acceleration bx = new Acceleration(biasX,
11449                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11450         final Acceleration by = new Acceleration(biasY,
11451                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11452         final Acceleration bz = new Acceleration(biasZ,
11453                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11454 
11455         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11456         final double latitude = Math.toRadians(
11457                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11458         final double longitude = Math.toRadians(
11459                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11460         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11461         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11462         final NEDVelocity nedVelocity = new NEDVelocity();
11463         final ECEFPosition ecefPosition = new ECEFPosition();
11464         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11465         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11466                 ecefPosition, ecefVelocity);
11467         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11468                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11469         final double gravityNorm = gravity.getNorm();
11470 
11471         KnownGravityNormAccelerometerCalibrator calibrator =
11472                 new KnownGravityNormAccelerometerCalibrator(
11473                         gravityNorm, measurements,
11474                         bx, by, bz, this);
11475 
11476         // check default values
11477         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11478         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11479         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11480         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11481         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11482         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11483         final Acceleration bx2 = new Acceleration(0.0,
11484                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11485         calibrator.getInitialBiasXAsAcceleration(bx2);
11486         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11487         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11488         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11489         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11490         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11491         final Acceleration by2 = new Acceleration(0.0,
11492                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11493         calibrator.getInitialBiasYAsAcceleration(by2);
11494         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11495         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11496         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11497         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11498         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11499         final Acceleration bz2 = new Acceleration(0.0,
11500                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11501         calibrator.getInitialBiasZAsAcceleration(bz2);
11502         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11503         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11504         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11505         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11506         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11507         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11508         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11509         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11510         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11511         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11512         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11513         final double[] bias1 = calibrator.getInitialBias();
11514         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11515         final double[] bias2 = new double[3];
11516         calibrator.getInitialBias(bias2);
11517         assertArrayEquals(bias1, bias2, 0.0);
11518         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11519         assertEquals(b1, ba);
11520         final Matrix b2 = new Matrix(3, 1);
11521         calibrator.getInitialBiasAsMatrix(b2);
11522         assertEquals(b1, b2);
11523         final Matrix ma1 = calibrator.getInitialMa();
11524         assertEquals(ma1, new Matrix(3, 3));
11525         final Matrix ma2 = new Matrix(3, 3);
11526         calibrator.getInitialMa(ma2);
11527         assertEquals(ma1, ma2);
11528         assertSame(calibrator.getMeasurements(), measurements);
11529         assertFalse(calibrator.isCommonAxisUsed());
11530         assertSame(calibrator.getListener(), this);
11531         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11532         assertFalse(calibrator.isReady());
11533         assertFalse(calibrator.isRunning());
11534         assertNull(calibrator.getEstimatedBiases());
11535         assertFalse(calibrator.getEstimatedBiases(null));
11536         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11537         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11538         assertNull(calibrator.getEstimatedBiasFx());
11539         assertNull(calibrator.getEstimatedBiasFy());
11540         assertNull(calibrator.getEstimatedBiasFz());
11541         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11542         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11543         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11544         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11545         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11546         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11547         assertNull(calibrator.getEstimatedMa());
11548         assertNull(calibrator.getEstimatedSx());
11549         assertNull(calibrator.getEstimatedSy());
11550         assertNull(calibrator.getEstimatedSz());
11551         assertNull(calibrator.getEstimatedMxy());
11552         assertNull(calibrator.getEstimatedMxz());
11553         assertNull(calibrator.getEstimatedMyx());
11554         assertNull(calibrator.getEstimatedMyz());
11555         assertNull(calibrator.getEstimatedMzx());
11556         assertNull(calibrator.getEstimatedMzy());
11557         assertNull(calibrator.getEstimatedCovariance());
11558         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11559         assertNotNull(calibrator.getGroundTruthGravityNorm());
11560         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11561         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11562         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11563                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11564         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11565         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11566         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11567 
11568         // Force IllegalArgumentException
11569         calibrator = null;
11570         try {
11571             calibrator = new KnownGravityNormAccelerometerCalibrator(
11572                     -gravityNorm, measurements,
11573                     bx, by, bz, this);
11574             fail("IllegalArgumentException expected but not thrown");
11575         } catch (final IllegalArgumentException ignore) {
11576         }
11577         assertNull(calibrator);
11578     }
11579 
11580     @Test
11581     public void testConstructor99() throws WrongSizeException {
11582         final Matrix ba = generateBa();
11583         final double biasX = ba.getElementAtIndex(0);
11584         final double biasY = ba.getElementAtIndex(1);
11585         final double biasZ = ba.getElementAtIndex(2);
11586 
11587         final Acceleration bx = new Acceleration(biasX,
11588                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11589         final Acceleration by = new Acceleration(biasY,
11590                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11591         final Acceleration bz = new Acceleration(biasZ,
11592                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11593 
11594         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11595         final double latitude = Math.toRadians(
11596                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11597         final double longitude = Math.toRadians(
11598                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11599         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11600         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11601         final NEDVelocity nedVelocity = new NEDVelocity();
11602         final ECEFPosition ecefPosition = new ECEFPosition();
11603         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11604         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11605                 ecefPosition, ecefVelocity);
11606         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11607                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11608         final double gravityNorm = gravity.getNorm();
11609 
11610         KnownGravityNormAccelerometerCalibrator calibrator =
11611                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
11612                         true, bx, by, bz);
11613 
11614         // check default values
11615         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11616         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11617         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11618         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11619         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11620         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11621         final Acceleration bx2 = new Acceleration(0.0,
11622                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11623         calibrator.getInitialBiasXAsAcceleration(bx2);
11624         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11625         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11626         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11627         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11628         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11629         final Acceleration by2 = new Acceleration(0.0,
11630                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11631         calibrator.getInitialBiasYAsAcceleration(by2);
11632         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11633         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11634         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11635         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11636         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11637         final Acceleration bz2 = new Acceleration(0.0,
11638                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11639         calibrator.getInitialBiasZAsAcceleration(bz2);
11640         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11641         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11642         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11643         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11644         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11645         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11646         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11647         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11648         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11649         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11650         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11651         final double[] bias1 = calibrator.getInitialBias();
11652         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11653         final double[] bias2 = new double[3];
11654         calibrator.getInitialBias(bias2);
11655         assertArrayEquals(bias1, bias2, 0.0);
11656         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11657         assertEquals(b1, ba);
11658         final Matrix b2 = new Matrix(3, 1);
11659         calibrator.getInitialBiasAsMatrix(b2);
11660         assertEquals(b1, b2);
11661         final Matrix ma1 = calibrator.getInitialMa();
11662         assertEquals(ma1, new Matrix(3, 3));
11663         final Matrix ma2 = new Matrix(3, 3);
11664         calibrator.getInitialMa(ma2);
11665         assertEquals(ma1, ma2);
11666         assertNull(calibrator.getMeasurements());
11667         assertTrue(calibrator.isCommonAxisUsed());
11668         assertNull(calibrator.getListener());
11669         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11670         assertFalse(calibrator.isReady());
11671         assertFalse(calibrator.isRunning());
11672         assertNull(calibrator.getEstimatedBiases());
11673         assertFalse(calibrator.getEstimatedBiases(null));
11674         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11675         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11676         assertNull(calibrator.getEstimatedBiasFx());
11677         assertNull(calibrator.getEstimatedBiasFy());
11678         assertNull(calibrator.getEstimatedBiasFz());
11679         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11680         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11681         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11682         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11683         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11684         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11685         assertNull(calibrator.getEstimatedMa());
11686         assertNull(calibrator.getEstimatedSx());
11687         assertNull(calibrator.getEstimatedSy());
11688         assertNull(calibrator.getEstimatedSz());
11689         assertNull(calibrator.getEstimatedMxy());
11690         assertNull(calibrator.getEstimatedMxz());
11691         assertNull(calibrator.getEstimatedMyx());
11692         assertNull(calibrator.getEstimatedMyz());
11693         assertNull(calibrator.getEstimatedMzx());
11694         assertNull(calibrator.getEstimatedMzy());
11695         assertNull(calibrator.getEstimatedCovariance());
11696         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11697         assertNotNull(calibrator.getGroundTruthGravityNorm());
11698         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11699         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11700         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11701                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11702         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11703         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11704         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11705 
11706         // Force IllegalArgumentException
11707         calibrator = null;
11708         try {
11709             calibrator = new KnownGravityNormAccelerometerCalibrator(
11710                     -gravityNorm, true, bx, by, bz);
11711             fail("IllegalArgumentException expected but not thrown");
11712         } catch (final IllegalArgumentException ignore) {
11713         }
11714         assertNull(calibrator);
11715     }
11716 
11717     @Test
11718     public void testConstructor100() throws WrongSizeException {
11719         final Matrix ba = generateBa();
11720         final double biasX = ba.getElementAtIndex(0);
11721         final double biasY = ba.getElementAtIndex(1);
11722         final double biasZ = ba.getElementAtIndex(2);
11723 
11724         final Acceleration bx = new Acceleration(biasX,
11725                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11726         final Acceleration by = new Acceleration(biasY,
11727                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11728         final Acceleration bz = new Acceleration(biasZ,
11729                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11730 
11731         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11732         final double latitude = Math.toRadians(
11733                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11734         final double longitude = Math.toRadians(
11735                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11736         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11737         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11738         final NEDVelocity nedVelocity = new NEDVelocity();
11739         final ECEFPosition ecefPosition = new ECEFPosition();
11740         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11741         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11742                 ecefPosition, ecefVelocity);
11743         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11744                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11745         final double gravityNorm = gravity.getNorm();
11746 
11747         KnownGravityNormAccelerometerCalibrator calibrator =
11748                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
11749                         true, bx, by, bz, this);
11750 
11751         // check default values
11752         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11753         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11754         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11755         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11756         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11757         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11758         final Acceleration bx2 = new Acceleration(0.0,
11759                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11760         calibrator.getInitialBiasXAsAcceleration(bx2);
11761         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11762         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11763         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11764         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11765         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11766         final Acceleration by2 = new Acceleration(0.0,
11767                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11768         calibrator.getInitialBiasYAsAcceleration(by2);
11769         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11770         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11771         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11772         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11773         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11774         final Acceleration bz2 = new Acceleration(0.0,
11775                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11776         calibrator.getInitialBiasZAsAcceleration(bz2);
11777         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11778         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11779         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11780         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11781         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11782         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11783         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11784         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11785         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11786         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11787         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11788         final double[] bias1 = calibrator.getInitialBias();
11789         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11790         final double[] bias2 = new double[3];
11791         calibrator.getInitialBias(bias2);
11792         assertArrayEquals(bias1, bias2, 0.0);
11793         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11794         assertEquals(b1, ba);
11795         final Matrix b2 = new Matrix(3, 1);
11796         calibrator.getInitialBiasAsMatrix(b2);
11797         assertEquals(b1, b2);
11798         final Matrix ma1 = calibrator.getInitialMa();
11799         assertEquals(ma1, new Matrix(3, 3));
11800         final Matrix ma2 = new Matrix(3, 3);
11801         calibrator.getInitialMa(ma2);
11802         assertEquals(ma1, ma2);
11803         assertNull(calibrator.getMeasurements());
11804         assertTrue(calibrator.isCommonAxisUsed());
11805         assertSame(calibrator.getListener(), this);
11806         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11807         assertFalse(calibrator.isReady());
11808         assertFalse(calibrator.isRunning());
11809         assertNull(calibrator.getEstimatedBiases());
11810         assertFalse(calibrator.getEstimatedBiases(null));
11811         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11812         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11813         assertNull(calibrator.getEstimatedBiasFx());
11814         assertNull(calibrator.getEstimatedBiasFy());
11815         assertNull(calibrator.getEstimatedBiasFz());
11816         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11817         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11818         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11819         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11820         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11821         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11822         assertNull(calibrator.getEstimatedMa());
11823         assertNull(calibrator.getEstimatedSx());
11824         assertNull(calibrator.getEstimatedSy());
11825         assertNull(calibrator.getEstimatedSz());
11826         assertNull(calibrator.getEstimatedMxy());
11827         assertNull(calibrator.getEstimatedMxz());
11828         assertNull(calibrator.getEstimatedMyx());
11829         assertNull(calibrator.getEstimatedMyz());
11830         assertNull(calibrator.getEstimatedMzx());
11831         assertNull(calibrator.getEstimatedMzy());
11832         assertNull(calibrator.getEstimatedCovariance());
11833         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11834         assertNotNull(calibrator.getGroundTruthGravityNorm());
11835         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11836         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11837         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11838                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11839         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11840         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11841         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11842 
11843         // Force IllegalArgumentException
11844         calibrator = null;
11845         try {
11846             calibrator = new KnownGravityNormAccelerometerCalibrator(
11847                     -gravityNorm, true, bx, by, bz, this);
11848             fail("IllegalArgumentException expected but not thrown");
11849         } catch (final IllegalArgumentException ignore) {
11850         }
11851         assertNull(calibrator);
11852     }
11853 
11854     @Test
11855     public void testConstructor101() throws WrongSizeException {
11856         final Collection<StandardDeviationBodyKinematics> measurements =
11857                 Collections.emptyList();
11858 
11859         final Matrix ba = generateBa();
11860         final double biasX = ba.getElementAtIndex(0);
11861         final double biasY = ba.getElementAtIndex(1);
11862         final double biasZ = ba.getElementAtIndex(2);
11863 
11864         final Acceleration bx = new Acceleration(biasX,
11865                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11866         final Acceleration by = new Acceleration(biasY,
11867                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11868         final Acceleration bz = new Acceleration(biasZ,
11869                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11870 
11871         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11872         final double latitude = Math.toRadians(
11873                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11874         final double longitude = Math.toRadians(
11875                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11876         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11877         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11878         final NEDVelocity nedVelocity = new NEDVelocity();
11879         final ECEFPosition ecefPosition = new ECEFPosition();
11880         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11881         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11882                 ecefPosition, ecefVelocity);
11883         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11884                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11885         final double gravityNorm = gravity.getNorm();
11886 
11887         KnownGravityNormAccelerometerCalibrator calibrator =
11888                 new KnownGravityNormAccelerometerCalibrator(
11889                         gravityNorm, measurements,
11890                         true, bx, by, bz);
11891 
11892         // check default values
11893         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11894         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11895         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11896         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11897         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11898         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11899         final Acceleration bx2 = new Acceleration(0.0,
11900                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11901         calibrator.getInitialBiasXAsAcceleration(bx2);
11902         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11903         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11904         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11905         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11906         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11907         final Acceleration by2 = new Acceleration(0.0,
11908                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11909         calibrator.getInitialBiasYAsAcceleration(by2);
11910         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11911         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11912         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11913         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11914         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11915         final Acceleration bz2 = new Acceleration(0.0,
11916                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11917         calibrator.getInitialBiasZAsAcceleration(bz2);
11918         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11919         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11920         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11921         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11922         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11923         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11924         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11925         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11926         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11927         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11928         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11929         final double[] bias1 = calibrator.getInitialBias();
11930         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11931         final double[] bias2 = new double[3];
11932         calibrator.getInitialBias(bias2);
11933         assertArrayEquals(bias1, bias2, 0.0);
11934         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11935         assertEquals(b1, ba);
11936         final Matrix b2 = new Matrix(3, 1);
11937         calibrator.getInitialBiasAsMatrix(b2);
11938         assertEquals(b1, b2);
11939         final Matrix ma1 = calibrator.getInitialMa();
11940         assertEquals(ma1, new Matrix(3, 3));
11941         final Matrix ma2 = new Matrix(3, 3);
11942         calibrator.getInitialMa(ma2);
11943         assertEquals(ma1, ma2);
11944         assertSame(calibrator.getMeasurements(), measurements);
11945         assertTrue(calibrator.isCommonAxisUsed());
11946         assertNull(calibrator.getListener());
11947         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11948         assertFalse(calibrator.isReady());
11949         assertFalse(calibrator.isRunning());
11950         assertNull(calibrator.getEstimatedBiases());
11951         assertFalse(calibrator.getEstimatedBiases(null));
11952         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11953         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11954         assertNull(calibrator.getEstimatedBiasFx());
11955         assertNull(calibrator.getEstimatedBiasFy());
11956         assertNull(calibrator.getEstimatedBiasFz());
11957         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11958         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11959         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11960         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11961         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11962         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11963         assertNull(calibrator.getEstimatedMa());
11964         assertNull(calibrator.getEstimatedSx());
11965         assertNull(calibrator.getEstimatedSy());
11966         assertNull(calibrator.getEstimatedSz());
11967         assertNull(calibrator.getEstimatedMxy());
11968         assertNull(calibrator.getEstimatedMxz());
11969         assertNull(calibrator.getEstimatedMyx());
11970         assertNull(calibrator.getEstimatedMyz());
11971         assertNull(calibrator.getEstimatedMzx());
11972         assertNull(calibrator.getEstimatedMzy());
11973         assertNull(calibrator.getEstimatedCovariance());
11974         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11975         assertNotNull(calibrator.getGroundTruthGravityNorm());
11976         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11977         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11978         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11979                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11980         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11981         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11982         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11983 
11984         // Force IllegalArgumentException
11985         calibrator = null;
11986         try {
11987             calibrator = new KnownGravityNormAccelerometerCalibrator(
11988                     -gravityNorm, measurements,
11989                     true, bx, by, bz);
11990             fail("IllegalArgumentException expected but not thrown");
11991         } catch (final IllegalArgumentException ignore) {
11992         }
11993         assertNull(calibrator);
11994     }
11995 
11996     @Test
11997     public void testConstructor102() throws WrongSizeException {
11998         final Collection<StandardDeviationBodyKinematics> measurements =
11999                 Collections.emptyList();
12000 
12001         final Matrix ba = generateBa();
12002         final double biasX = ba.getElementAtIndex(0);
12003         final double biasY = ba.getElementAtIndex(1);
12004         final double biasZ = ba.getElementAtIndex(2);
12005 
12006         final Acceleration bx = new Acceleration(biasX,
12007                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12008         final Acceleration by = new Acceleration(biasY,
12009                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12010         final Acceleration bz = new Acceleration(biasZ,
12011                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12012 
12013         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12014         final double latitude = Math.toRadians(
12015                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12016         final double longitude = Math.toRadians(
12017                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12018         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12019         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12020         final NEDVelocity nedVelocity = new NEDVelocity();
12021         final ECEFPosition ecefPosition = new ECEFPosition();
12022         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12023         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12024                 ecefPosition, ecefVelocity);
12025         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12026                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12027         final double gravityNorm = gravity.getNorm();
12028 
12029         KnownGravityNormAccelerometerCalibrator calibrator =
12030                 new KnownGravityNormAccelerometerCalibrator(
12031                         gravityNorm, measurements,
12032                         true, bx, by, bz, this);
12033 
12034         // check default values
12035         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12036         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12037         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12038         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12039         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12040         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12041         final Acceleration bx2 = new Acceleration(0.0,
12042                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12043         calibrator.getInitialBiasXAsAcceleration(bx2);
12044         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12045         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12046         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12047         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12048         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12049         final Acceleration by2 = new Acceleration(0.0,
12050                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12051         calibrator.getInitialBiasYAsAcceleration(by2);
12052         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12053         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12054         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12055         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12056         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12057         final Acceleration bz2 = new Acceleration(0.0,
12058                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12059         calibrator.getInitialBiasZAsAcceleration(bz2);
12060         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12061         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12062         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
12063         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
12064         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
12065         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12066         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12067         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12068         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12069         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12070         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12071         final double[] bias1 = calibrator.getInitialBias();
12072         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12073         final double[] bias2 = new double[3];
12074         calibrator.getInitialBias(bias2);
12075         assertArrayEquals(bias1, bias2, 0.0);
12076         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12077         assertEquals(b1, ba);
12078         final Matrix b2 = new Matrix(3, 1);
12079         calibrator.getInitialBiasAsMatrix(b2);
12080         assertEquals(b1, b2);
12081         final Matrix ma1 = calibrator.getInitialMa();
12082         assertEquals(ma1, new Matrix(3, 3));
12083         final Matrix ma2 = new Matrix(3, 3);
12084         calibrator.getInitialMa(ma2);
12085         assertEquals(ma1, ma2);
12086         assertSame(calibrator.getMeasurements(), measurements);
12087         assertTrue(calibrator.isCommonAxisUsed());
12088         assertSame(calibrator.getListener(), this);
12089         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12090         assertFalse(calibrator.isReady());
12091         assertFalse(calibrator.isRunning());
12092         assertNull(calibrator.getEstimatedBiases());
12093         assertFalse(calibrator.getEstimatedBiases(null));
12094         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12095         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12096         assertNull(calibrator.getEstimatedBiasFx());
12097         assertNull(calibrator.getEstimatedBiasFy());
12098         assertNull(calibrator.getEstimatedBiasFz());
12099         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12100         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12101         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12102         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12103         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12104         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12105         assertNull(calibrator.getEstimatedMa());
12106         assertNull(calibrator.getEstimatedSx());
12107         assertNull(calibrator.getEstimatedSy());
12108         assertNull(calibrator.getEstimatedSz());
12109         assertNull(calibrator.getEstimatedMxy());
12110         assertNull(calibrator.getEstimatedMxz());
12111         assertNull(calibrator.getEstimatedMyx());
12112         assertNull(calibrator.getEstimatedMyz());
12113         assertNull(calibrator.getEstimatedMzx());
12114         assertNull(calibrator.getEstimatedMzy());
12115         assertNull(calibrator.getEstimatedCovariance());
12116         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12117         assertNotNull(calibrator.getGroundTruthGravityNorm());
12118         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12119         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12120         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12121                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12122         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12123         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12124         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12125 
12126         // Force IllegalArgumentException
12127         calibrator = null;
12128         try {
12129             calibrator = new KnownGravityNormAccelerometerCalibrator(
12130                     -gravityNorm, measurements,
12131                     true, bx, by, bz, this);
12132             fail("IllegalArgumentException expected but not thrown");
12133         } catch (final IllegalArgumentException ignore) {
12134         }
12135         assertNull(calibrator);
12136     }
12137 
12138     @Test
12139     public void testConstructor103() throws WrongSizeException {
12140         final Matrix ba = generateBa();
12141         final double biasX = ba.getElementAtIndex(0);
12142         final double biasY = ba.getElementAtIndex(1);
12143         final double biasZ = ba.getElementAtIndex(2);
12144 
12145         final Matrix ma = generateMaCommonAxis();
12146         final double sx = ma.getElementAt(0, 0);
12147         final double sy = ma.getElementAt(1, 1);
12148         final double sz = ma.getElementAt(2, 2);
12149 
12150         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12151         final double latitude = Math.toRadians(
12152                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12153         final double longitude = Math.toRadians(
12154                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12155         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12156         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12157         final NEDVelocity nedVelocity = new NEDVelocity();
12158         final ECEFPosition ecefPosition = new ECEFPosition();
12159         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12160         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12161                 ecefPosition, ecefVelocity);
12162         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12163                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12164         final double gravityNorm = gravity.getNorm();
12165 
12166         KnownGravityNormAccelerometerCalibrator calibrator =
12167                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
12168                         biasX, biasY, biasZ, sx, sy, sz);
12169 
12170         // check default values
12171         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12172         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12173         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12174         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12175         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12176         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12177         final Acceleration bx2 = new Acceleration(0.0,
12178                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12179         calibrator.getInitialBiasXAsAcceleration(bx2);
12180         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12181         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12182         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12183         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12184         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12185         final Acceleration by2 = new Acceleration(0.0,
12186                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12187         calibrator.getInitialBiasYAsAcceleration(by2);
12188         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12189         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12190         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12191         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12192         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12193         final Acceleration bz2 = new Acceleration(0.0,
12194                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12195         calibrator.getInitialBiasZAsAcceleration(bz2);
12196         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12197         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12198         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12199         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12200         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12201         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12202         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12203         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12204         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12205         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12206         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12207         final double[] bias1 = calibrator.getInitialBias();
12208         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12209         final double[] bias2 = new double[3];
12210         calibrator.getInitialBias(bias2);
12211         assertArrayEquals(bias1, bias2, 0.0);
12212         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12213         assertEquals(b1, ba);
12214         final Matrix b2 = new Matrix(3, 1);
12215         calibrator.getInitialBiasAsMatrix(b2);
12216         assertEquals(b1, b2);
12217         final Matrix ma1 = calibrator.getInitialMa();
12218         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12219         final Matrix ma2 = new Matrix(3, 3);
12220         calibrator.getInitialMa(ma2);
12221         assertEquals(ma1, ma2);
12222         assertNull(calibrator.getMeasurements());
12223         assertFalse(calibrator.isCommonAxisUsed());
12224         assertNull(calibrator.getListener());
12225         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12226         assertFalse(calibrator.isReady());
12227         assertFalse(calibrator.isRunning());
12228         assertNull(calibrator.getEstimatedBiases());
12229         assertFalse(calibrator.getEstimatedBiases(null));
12230         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12231         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12232         assertNull(calibrator.getEstimatedBiasFx());
12233         assertNull(calibrator.getEstimatedBiasFy());
12234         assertNull(calibrator.getEstimatedBiasFz());
12235         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12236         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12237         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12238         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12239         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12240         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12241         assertNull(calibrator.getEstimatedMa());
12242         assertNull(calibrator.getEstimatedSx());
12243         assertNull(calibrator.getEstimatedSy());
12244         assertNull(calibrator.getEstimatedSz());
12245         assertNull(calibrator.getEstimatedMxy());
12246         assertNull(calibrator.getEstimatedMxz());
12247         assertNull(calibrator.getEstimatedMyx());
12248         assertNull(calibrator.getEstimatedMyz());
12249         assertNull(calibrator.getEstimatedMzx());
12250         assertNull(calibrator.getEstimatedMzy());
12251         assertNull(calibrator.getEstimatedCovariance());
12252         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12253         assertNotNull(calibrator.getGroundTruthGravityNorm());
12254         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12255         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12256         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12257                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12258         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12259         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12260         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12261 
12262         // Force IllegalArgumentException
12263         calibrator = null;
12264         try {
12265             calibrator = new KnownGravityNormAccelerometerCalibrator(
12266                     -gravityNorm, biasX, biasY, biasZ, sx, sy, sz);
12267             fail("IllegalArgumentException expected but not thrown");
12268         } catch (final IllegalArgumentException ignore) {
12269         }
12270         assertNull(calibrator);
12271     }
12272 
12273     @Test
12274     public void testConstructor104() throws WrongSizeException {
12275         final Collection<StandardDeviationBodyKinematics> measurements =
12276                 Collections.emptyList();
12277 
12278         final Matrix ba = generateBa();
12279         final double biasX = ba.getElementAtIndex(0);
12280         final double biasY = ba.getElementAtIndex(1);
12281         final double biasZ = ba.getElementAtIndex(2);
12282 
12283         final Matrix ma = generateMaCommonAxis();
12284         final double sx = ma.getElementAt(0, 0);
12285         final double sy = ma.getElementAt(1, 1);
12286         final double sz = ma.getElementAt(2, 2);
12287 
12288         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12289         final double latitude = Math.toRadians(
12290                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12291         final double longitude = Math.toRadians(
12292                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12293         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12294         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12295         final NEDVelocity nedVelocity = new NEDVelocity();
12296         final ECEFPosition ecefPosition = new ECEFPosition();
12297         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12298         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12299                 ecefPosition, ecefVelocity);
12300         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12301                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12302         final double gravityNorm = gravity.getNorm();
12303 
12304         KnownGravityNormAccelerometerCalibrator calibrator =
12305                 new KnownGravityNormAccelerometerCalibrator(
12306                         gravityNorm, measurements,
12307                         biasX, biasY, biasZ, sx, sy, sz);
12308 
12309         // check default values
12310         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12311         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12312         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12313         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12314         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12315         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12316         final Acceleration bx2 = new Acceleration(0.0,
12317                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12318         calibrator.getInitialBiasXAsAcceleration(bx2);
12319         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12320         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12321         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12322         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12323         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12324         final Acceleration by2 = new Acceleration(0.0,
12325                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12326         calibrator.getInitialBiasYAsAcceleration(by2);
12327         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12328         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12329         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12330         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12331         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12332         final Acceleration bz2 = new Acceleration(0.0,
12333                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12334         calibrator.getInitialBiasZAsAcceleration(bz2);
12335         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12336         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12337         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12338         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12339         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12340         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12341         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12342         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12343         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12344         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12345         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12346         final double[] bias1 = calibrator.getInitialBias();
12347         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12348         final double[] bias2 = new double[3];
12349         calibrator.getInitialBias(bias2);
12350         assertArrayEquals(bias1, bias2, 0.0);
12351         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12352         assertEquals(b1, ba);
12353         final Matrix b2 = new Matrix(3, 1);
12354         calibrator.getInitialBiasAsMatrix(b2);
12355         assertEquals(b1, b2);
12356         final Matrix ma1 = calibrator.getInitialMa();
12357         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12358         final Matrix ma2 = new Matrix(3, 3);
12359         calibrator.getInitialMa(ma2);
12360         assertEquals(ma1, ma2);
12361         assertSame(calibrator.getMeasurements(), measurements);
12362         assertFalse(calibrator.isCommonAxisUsed());
12363         assertNull(calibrator.getListener());
12364         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12365         assertFalse(calibrator.isReady());
12366         assertFalse(calibrator.isRunning());
12367         assertNull(calibrator.getEstimatedBiases());
12368         assertFalse(calibrator.getEstimatedBiases(null));
12369         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12370         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12371         assertNull(calibrator.getEstimatedBiasFx());
12372         assertNull(calibrator.getEstimatedBiasFy());
12373         assertNull(calibrator.getEstimatedBiasFz());
12374         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12375         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12376         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12377         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12378         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12379         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12380         assertNull(calibrator.getEstimatedMa());
12381         assertNull(calibrator.getEstimatedSx());
12382         assertNull(calibrator.getEstimatedSy());
12383         assertNull(calibrator.getEstimatedSz());
12384         assertNull(calibrator.getEstimatedMxy());
12385         assertNull(calibrator.getEstimatedMxz());
12386         assertNull(calibrator.getEstimatedMyx());
12387         assertNull(calibrator.getEstimatedMyz());
12388         assertNull(calibrator.getEstimatedMzx());
12389         assertNull(calibrator.getEstimatedMzy());
12390         assertNull(calibrator.getEstimatedCovariance());
12391         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12392         assertNotNull(calibrator.getGroundTruthGravityNorm());
12393         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12394         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12395         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12396                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12397         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12398         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12399         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12400 
12401         // Force IllegalArgumentException
12402         calibrator = null;
12403         try {
12404             calibrator = new KnownGravityNormAccelerometerCalibrator(
12405                     -gravityNorm, measurements,
12406                     biasX, biasY, biasZ, sx, sy, sz);
12407             fail("IllegalArgumentException expected but not thrown");
12408         } catch (final IllegalArgumentException ignore) {
12409         }
12410         assertNull(calibrator);
12411     }
12412 
12413     @Test
12414     public void testConstructor105() throws WrongSizeException {
12415         final Collection<StandardDeviationBodyKinematics> measurements =
12416                 Collections.emptyList();
12417 
12418         final Matrix ba = generateBa();
12419         final double biasX = ba.getElementAtIndex(0);
12420         final double biasY = ba.getElementAtIndex(1);
12421         final double biasZ = ba.getElementAtIndex(2);
12422 
12423         final Matrix ma = generateMaCommonAxis();
12424         final double sx = ma.getElementAt(0, 0);
12425         final double sy = ma.getElementAt(1, 1);
12426         final double sz = ma.getElementAt(2, 2);
12427 
12428         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12429         final double latitude = Math.toRadians(
12430                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12431         final double longitude = Math.toRadians(
12432                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12433         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12434         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12435         final NEDVelocity nedVelocity = new NEDVelocity();
12436         final ECEFPosition ecefPosition = new ECEFPosition();
12437         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12438         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12439                 ecefPosition, ecefVelocity);
12440         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12441                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12442         final double gravityNorm = gravity.getNorm();
12443 
12444         KnownGravityNormAccelerometerCalibrator calibrator =
12445                 new KnownGravityNormAccelerometerCalibrator(
12446                         gravityNorm, measurements,
12447                         biasX, biasY, biasZ, sx, sy, sz, this);
12448 
12449         // check default values
12450         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12451         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12452         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12453         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12454         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12455         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12456         final Acceleration bx2 = new Acceleration(0.0,
12457                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12458         calibrator.getInitialBiasXAsAcceleration(bx2);
12459         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12460         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12461         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12462         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12463         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12464         final Acceleration by2 = new Acceleration(0.0,
12465                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12466         calibrator.getInitialBiasYAsAcceleration(by2);
12467         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12468         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12469         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12470         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12471         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12472         final Acceleration bz2 = new Acceleration(0.0,
12473                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12474         calibrator.getInitialBiasZAsAcceleration(bz2);
12475         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12476         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12477         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12478         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12479         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12480         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12481         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12482         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12483         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12484         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12485         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12486         final double[] bias1 = calibrator.getInitialBias();
12487         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12488         final double[] bias2 = new double[3];
12489         calibrator.getInitialBias(bias2);
12490         assertArrayEquals(bias1, bias2, 0.0);
12491         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12492         assertEquals(b1, ba);
12493         final Matrix b2 = new Matrix(3, 1);
12494         calibrator.getInitialBiasAsMatrix(b2);
12495         assertEquals(b1, b2);
12496         final Matrix ma1 = calibrator.getInitialMa();
12497         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12498         final Matrix ma2 = new Matrix(3, 3);
12499         calibrator.getInitialMa(ma2);
12500         assertEquals(ma1, ma2);
12501         assertSame(calibrator.getMeasurements(), measurements);
12502         assertFalse(calibrator.isCommonAxisUsed());
12503         assertSame(calibrator.getListener(), this);
12504         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12505         assertFalse(calibrator.isReady());
12506         assertFalse(calibrator.isRunning());
12507         assertNull(calibrator.getEstimatedBiases());
12508         assertFalse(calibrator.getEstimatedBiases(null));
12509         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12510         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12511         assertNull(calibrator.getEstimatedBiasFx());
12512         assertNull(calibrator.getEstimatedBiasFy());
12513         assertNull(calibrator.getEstimatedBiasFz());
12514         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12515         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12516         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12517         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12518         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12519         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12520         assertNull(calibrator.getEstimatedMa());
12521         assertNull(calibrator.getEstimatedSx());
12522         assertNull(calibrator.getEstimatedSy());
12523         assertNull(calibrator.getEstimatedSz());
12524         assertNull(calibrator.getEstimatedMxy());
12525         assertNull(calibrator.getEstimatedMxz());
12526         assertNull(calibrator.getEstimatedMyx());
12527         assertNull(calibrator.getEstimatedMyz());
12528         assertNull(calibrator.getEstimatedMzx());
12529         assertNull(calibrator.getEstimatedMzy());
12530         assertNull(calibrator.getEstimatedCovariance());
12531         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12532         assertNotNull(calibrator.getGroundTruthGravityNorm());
12533         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12534         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12535         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12536                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12537         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12538         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12539         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12540 
12541         // Force IllegalArgumentException
12542         calibrator = null;
12543         try {
12544             calibrator = new KnownGravityNormAccelerometerCalibrator(
12545                     -gravityNorm, measurements,
12546                     biasX, biasY, biasZ, sx, sy, sz, this);
12547             fail("IllegalArgumentException expected but not thrown");
12548         } catch (final IllegalArgumentException ignore) {
12549         }
12550         assertNull(calibrator);
12551     }
12552 
12553     @Test
12554     public void testConstructor106() throws WrongSizeException {
12555         final Matrix ba = generateBa();
12556         final double biasX = ba.getElementAtIndex(0);
12557         final double biasY = ba.getElementAtIndex(1);
12558         final double biasZ = ba.getElementAtIndex(2);
12559 
12560         final Matrix ma = generateMaCommonAxis();
12561         final double sx = ma.getElementAt(0, 0);
12562         final double sy = ma.getElementAt(1, 1);
12563         final double sz = ma.getElementAt(2, 2);
12564 
12565         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12566         final double latitude = Math.toRadians(
12567                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12568         final double longitude = Math.toRadians(
12569                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12570         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12571         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12572         final NEDVelocity nedVelocity = new NEDVelocity();
12573         final ECEFPosition ecefPosition = new ECEFPosition();
12574         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12575         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12576                 ecefPosition, ecefVelocity);
12577         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12578                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12579         final double gravityNorm = gravity.getNorm();
12580 
12581         KnownGravityNormAccelerometerCalibrator calibrator =
12582                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
12583                         true, biasX, biasY, biasZ,
12584                         sx, sy, sz);
12585 
12586         // check default values
12587         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12588         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12589         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12590         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12591         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12592         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12593         final Acceleration bx2 = new Acceleration(0.0,
12594                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12595         calibrator.getInitialBiasXAsAcceleration(bx2);
12596         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12597         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12598         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12599         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12600         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12601         final Acceleration by2 = new Acceleration(0.0,
12602                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12603         calibrator.getInitialBiasYAsAcceleration(by2);
12604         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12605         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12606         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12607         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12608         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12609         final Acceleration bz2 = new Acceleration(0.0,
12610                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12611         calibrator.getInitialBiasZAsAcceleration(bz2);
12612         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12613         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12614         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12615         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12616         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12617         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12618         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12619         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12620         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12621         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12622         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12623         final double[] bias1 = calibrator.getInitialBias();
12624         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12625         final double[] bias2 = new double[3];
12626         calibrator.getInitialBias(bias2);
12627         assertArrayEquals(bias1, bias2, 0.0);
12628         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12629         assertEquals(b1, ba);
12630         final Matrix b2 = new Matrix(3, 1);
12631         calibrator.getInitialBiasAsMatrix(b2);
12632         assertEquals(b1, b2);
12633         final Matrix ma1 = calibrator.getInitialMa();
12634         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12635         final Matrix ma2 = new Matrix(3, 3);
12636         calibrator.getInitialMa(ma2);
12637         assertEquals(ma1, ma2);
12638         assertNull(calibrator.getMeasurements());
12639         assertTrue(calibrator.isCommonAxisUsed());
12640         assertNull(calibrator.getListener());
12641         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12642         assertFalse(calibrator.isReady());
12643         assertFalse(calibrator.isRunning());
12644         assertNull(calibrator.getEstimatedBiases());
12645         assertFalse(calibrator.getEstimatedBiases(null));
12646         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12647         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12648         assertNull(calibrator.getEstimatedBiasFx());
12649         assertNull(calibrator.getEstimatedBiasFy());
12650         assertNull(calibrator.getEstimatedBiasFz());
12651         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12652         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12653         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12654         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12655         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12656         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12657         assertNull(calibrator.getEstimatedMa());
12658         assertNull(calibrator.getEstimatedSx());
12659         assertNull(calibrator.getEstimatedSy());
12660         assertNull(calibrator.getEstimatedSz());
12661         assertNull(calibrator.getEstimatedMxy());
12662         assertNull(calibrator.getEstimatedMxz());
12663         assertNull(calibrator.getEstimatedMyx());
12664         assertNull(calibrator.getEstimatedMyz());
12665         assertNull(calibrator.getEstimatedMzx());
12666         assertNull(calibrator.getEstimatedMzy());
12667         assertNull(calibrator.getEstimatedCovariance());
12668         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12669         assertNotNull(calibrator.getGroundTruthGravityNorm());
12670         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12671         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12672         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12673                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12674         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12675         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12676         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12677 
12678         // Force IllegalArgumentException
12679         calibrator = null;
12680         try {
12681             calibrator = new KnownGravityNormAccelerometerCalibrator(
12682                     -gravityNorm, true, biasX, biasY, biasZ, sx, sy, sz);
12683             fail("IllegalArgumentException expected but not thrown");
12684         } catch (final IllegalArgumentException ignore) {
12685         }
12686         assertNull(calibrator);
12687     }
12688 
12689     @Test
12690     public void testConstructor107() throws WrongSizeException {
12691         final Matrix ba = generateBa();
12692         final double biasX = ba.getElementAtIndex(0);
12693         final double biasY = ba.getElementAtIndex(1);
12694         final double biasZ = ba.getElementAtIndex(2);
12695 
12696         final Matrix ma = generateMaCommonAxis();
12697         final double sx = ma.getElementAt(0, 0);
12698         final double sy = ma.getElementAt(1, 1);
12699         final double sz = ma.getElementAt(2, 2);
12700 
12701         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12702         final double latitude = Math.toRadians(
12703                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12704         final double longitude = Math.toRadians(
12705                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12706         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12707         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12708         final NEDVelocity nedVelocity = new NEDVelocity();
12709         final ECEFPosition ecefPosition = new ECEFPosition();
12710         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12711         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12712                 ecefPosition, ecefVelocity);
12713         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12714                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12715         final double gravityNorm = gravity.getNorm();
12716 
12717         KnownGravityNormAccelerometerCalibrator calibrator =
12718                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
12719                         true, biasX, biasY, biasZ, sx, sy, sz,
12720                         this);
12721 
12722         // check default values
12723         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12724         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12725         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12726         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12727         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12728         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12729         final Acceleration bx2 = new Acceleration(0.0,
12730                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12731         calibrator.getInitialBiasXAsAcceleration(bx2);
12732         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12733         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12734         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12735         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12736         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12737         final Acceleration by2 = new Acceleration(0.0,
12738                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12739         calibrator.getInitialBiasYAsAcceleration(by2);
12740         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12741         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12742         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12743         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12744         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12745         final Acceleration bz2 = new Acceleration(0.0,
12746                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12747         calibrator.getInitialBiasZAsAcceleration(bz2);
12748         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12749         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12750         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12751         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12752         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12753         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12754         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12755         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12756         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12757         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12758         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12759         final double[] bias1 = calibrator.getInitialBias();
12760         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12761         final double[] bias2 = new double[3];
12762         calibrator.getInitialBias(bias2);
12763         assertArrayEquals(bias1, bias2, 0.0);
12764         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12765         assertEquals(b1, ba);
12766         final Matrix b2 = new Matrix(3, 1);
12767         calibrator.getInitialBiasAsMatrix(b2);
12768         assertEquals(b1, b2);
12769         final Matrix ma1 = calibrator.getInitialMa();
12770         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12771         final Matrix ma2 = new Matrix(3, 3);
12772         calibrator.getInitialMa(ma2);
12773         assertEquals(ma1, ma2);
12774         assertNull(calibrator.getMeasurements());
12775         assertTrue(calibrator.isCommonAxisUsed());
12776         assertSame(calibrator.getListener(), this);
12777         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12778         assertFalse(calibrator.isReady());
12779         assertFalse(calibrator.isRunning());
12780         assertNull(calibrator.getEstimatedBiases());
12781         assertFalse(calibrator.getEstimatedBiases(null));
12782         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12783         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12784         assertNull(calibrator.getEstimatedBiasFx());
12785         assertNull(calibrator.getEstimatedBiasFy());
12786         assertNull(calibrator.getEstimatedBiasFz());
12787         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12788         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12789         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12790         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12791         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12792         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12793         assertNull(calibrator.getEstimatedMa());
12794         assertNull(calibrator.getEstimatedSx());
12795         assertNull(calibrator.getEstimatedSy());
12796         assertNull(calibrator.getEstimatedSz());
12797         assertNull(calibrator.getEstimatedMxy());
12798         assertNull(calibrator.getEstimatedMxz());
12799         assertNull(calibrator.getEstimatedMyx());
12800         assertNull(calibrator.getEstimatedMyz());
12801         assertNull(calibrator.getEstimatedMzx());
12802         assertNull(calibrator.getEstimatedMzy());
12803         assertNull(calibrator.getEstimatedCovariance());
12804         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12805         assertNotNull(calibrator.getGroundTruthGravityNorm());
12806         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12807         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12808         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12809                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12810         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12811         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12812         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12813 
12814         // Force IllegalArgumentException
12815         calibrator = null;
12816         try {
12817             calibrator = new KnownGravityNormAccelerometerCalibrator(
12818                     -gravityNorm, true, biasX, biasY, biasZ, sx, sy, sz,
12819                     this);
12820             fail("IllegalArgumentException expected but not thrown");
12821         } catch (final IllegalArgumentException ignore) {
12822         }
12823         assertNull(calibrator);
12824     }
12825 
12826     @Test
12827     public void testConstructor108() throws WrongSizeException {
12828         final Collection<StandardDeviationBodyKinematics> measurements =
12829                 Collections.emptyList();
12830 
12831         final Matrix ba = generateBa();
12832         final double biasX = ba.getElementAtIndex(0);
12833         final double biasY = ba.getElementAtIndex(1);
12834         final double biasZ = ba.getElementAtIndex(2);
12835 
12836         final Matrix ma = generateMaCommonAxis();
12837         final double sx = ma.getElementAt(0, 0);
12838         final double sy = ma.getElementAt(1, 1);
12839         final double sz = ma.getElementAt(2, 2);
12840 
12841         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12842         final double latitude = Math.toRadians(
12843                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12844         final double longitude = Math.toRadians(
12845                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12846         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12847         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12848         final NEDVelocity nedVelocity = new NEDVelocity();
12849         final ECEFPosition ecefPosition = new ECEFPosition();
12850         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12851         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12852                 ecefPosition, ecefVelocity);
12853         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12854                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12855         final double gravityNorm = gravity.getNorm();
12856 
12857         KnownGravityNormAccelerometerCalibrator calibrator =
12858                 new KnownGravityNormAccelerometerCalibrator(
12859                         gravityNorm, measurements,
12860                         true, biasX, biasY, biasZ,
12861                         sx, sy, sz);
12862 
12863         // check default values
12864         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12865         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12866         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12867         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12868         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12869         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12870         final Acceleration bx2 = new Acceleration(0.0,
12871                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12872         calibrator.getInitialBiasXAsAcceleration(bx2);
12873         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12874         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12875         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12876         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12877         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12878         final Acceleration by2 = new Acceleration(0.0,
12879                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12880         calibrator.getInitialBiasYAsAcceleration(by2);
12881         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12882         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12883         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12884         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12885         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12886         final Acceleration bz2 = new Acceleration(0.0,
12887                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12888         calibrator.getInitialBiasZAsAcceleration(bz2);
12889         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12890         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12891         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12892         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12893         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12894         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12895         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12896         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12897         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12898         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12899         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12900         final double[] bias1 = calibrator.getInitialBias();
12901         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12902         final double[] bias2 = new double[3];
12903         calibrator.getInitialBias(bias2);
12904         assertArrayEquals(bias1, bias2, 0.0);
12905         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12906         assertEquals(b1, ba);
12907         final Matrix b2 = new Matrix(3, 1);
12908         calibrator.getInitialBiasAsMatrix(b2);
12909         assertEquals(b1, b2);
12910         final Matrix ma1 = calibrator.getInitialMa();
12911         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12912         final Matrix ma2 = new Matrix(3, 3);
12913         calibrator.getInitialMa(ma2);
12914         assertEquals(ma1, ma2);
12915         assertSame(calibrator.getMeasurements(), measurements);
12916         assertTrue(calibrator.isCommonAxisUsed());
12917         assertNull(calibrator.getListener());
12918         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12919         assertFalse(calibrator.isReady());
12920         assertFalse(calibrator.isRunning());
12921         assertNull(calibrator.getEstimatedBiases());
12922         assertFalse(calibrator.getEstimatedBiases(null));
12923         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12924         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12925         assertNull(calibrator.getEstimatedBiasFx());
12926         assertNull(calibrator.getEstimatedBiasFy());
12927         assertNull(calibrator.getEstimatedBiasFz());
12928         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12929         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12930         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12931         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12932         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12933         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12934         assertNull(calibrator.getEstimatedMa());
12935         assertNull(calibrator.getEstimatedSx());
12936         assertNull(calibrator.getEstimatedSy());
12937         assertNull(calibrator.getEstimatedSz());
12938         assertNull(calibrator.getEstimatedMxy());
12939         assertNull(calibrator.getEstimatedMxz());
12940         assertNull(calibrator.getEstimatedMyx());
12941         assertNull(calibrator.getEstimatedMyz());
12942         assertNull(calibrator.getEstimatedMzx());
12943         assertNull(calibrator.getEstimatedMzy());
12944         assertNull(calibrator.getEstimatedCovariance());
12945         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12946         assertNotNull(calibrator.getGroundTruthGravityNorm());
12947         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12948         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12949         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12950                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12951         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12952         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12953         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12954 
12955         // Force IllegalArgumentException
12956         calibrator = null;
12957         try {
12958             calibrator = new KnownGravityNormAccelerometerCalibrator(
12959                     -gravityNorm, measurements,
12960                     true, biasX, biasY, biasZ,
12961                     sx, sy, sz);
12962             fail("IllegalArgumentException expected but not thrown");
12963         } catch (final IllegalArgumentException ignore) {
12964         }
12965         assertNull(calibrator);
12966     }
12967 
12968     @Test
12969     public void testConstructor109() throws WrongSizeException {
12970         final Collection<StandardDeviationBodyKinematics> measurements =
12971                 Collections.emptyList();
12972 
12973         final Matrix ba = generateBa();
12974         final double biasX = ba.getElementAtIndex(0);
12975         final double biasY = ba.getElementAtIndex(1);
12976         final double biasZ = ba.getElementAtIndex(2);
12977 
12978         final Matrix ma = generateMaCommonAxis();
12979         final double sx = ma.getElementAt(0, 0);
12980         final double sy = ma.getElementAt(1, 1);
12981         final double sz = ma.getElementAt(2, 2);
12982 
12983         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12984         final double latitude = Math.toRadians(
12985                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12986         final double longitude = Math.toRadians(
12987                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12988         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12989         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12990         final NEDVelocity nedVelocity = new NEDVelocity();
12991         final ECEFPosition ecefPosition = new ECEFPosition();
12992         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12993         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12994                 ecefPosition, ecefVelocity);
12995         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12996                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12997         final double gravityNorm = gravity.getNorm();
12998 
12999         KnownGravityNormAccelerometerCalibrator calibrator =
13000                 new KnownGravityNormAccelerometerCalibrator(
13001                         gravityNorm, measurements,
13002                         true, biasX, biasY, biasZ, sx, sy, sz,
13003                         this);
13004 
13005         // check default values
13006         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13007         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13008         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13009         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13010         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13011         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13012         final Acceleration bx2 = new Acceleration(0.0,
13013                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13014         calibrator.getInitialBiasXAsAcceleration(bx2);
13015         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13016         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13017         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13018         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13019         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13020         final Acceleration by2 = new Acceleration(0.0,
13021                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13022         calibrator.getInitialBiasYAsAcceleration(by2);
13023         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13024         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13025         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13026         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13027         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13028         final Acceleration bz2 = new Acceleration(0.0,
13029                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13030         calibrator.getInitialBiasZAsAcceleration(bz2);
13031         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13032         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13033         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13034         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13035         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13036         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13037         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13038         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13039         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13040         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13041         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13042         final double[] bias1 = calibrator.getInitialBias();
13043         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13044         final double[] bias2 = new double[3];
13045         calibrator.getInitialBias(bias2);
13046         assertArrayEquals(bias1, bias2, 0.0);
13047         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13048         assertEquals(b1, ba);
13049         final Matrix b2 = new Matrix(3, 1);
13050         calibrator.getInitialBiasAsMatrix(b2);
13051         assertEquals(b1, b2);
13052         final Matrix ma1 = calibrator.getInitialMa();
13053         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13054         final Matrix ma2 = new Matrix(3, 3);
13055         calibrator.getInitialMa(ma2);
13056         assertEquals(ma1, ma2);
13057         assertSame(calibrator.getMeasurements(), measurements);
13058         assertTrue(calibrator.isCommonAxisUsed());
13059         assertSame(calibrator.getListener(), this);
13060         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13061         assertFalse(calibrator.isReady());
13062         assertFalse(calibrator.isRunning());
13063         assertNull(calibrator.getEstimatedBiases());
13064         assertFalse(calibrator.getEstimatedBiases(null));
13065         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13066         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13067         assertNull(calibrator.getEstimatedBiasFx());
13068         assertNull(calibrator.getEstimatedBiasFy());
13069         assertNull(calibrator.getEstimatedBiasFz());
13070         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13071         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13072         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13073         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13074         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13075         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13076         assertNull(calibrator.getEstimatedMa());
13077         assertNull(calibrator.getEstimatedSx());
13078         assertNull(calibrator.getEstimatedSy());
13079         assertNull(calibrator.getEstimatedSz());
13080         assertNull(calibrator.getEstimatedMxy());
13081         assertNull(calibrator.getEstimatedMxz());
13082         assertNull(calibrator.getEstimatedMyx());
13083         assertNull(calibrator.getEstimatedMyz());
13084         assertNull(calibrator.getEstimatedMzx());
13085         assertNull(calibrator.getEstimatedMzy());
13086         assertNull(calibrator.getEstimatedCovariance());
13087         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13088         assertNotNull(calibrator.getGroundTruthGravityNorm());
13089         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13090         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13091         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13092                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13093         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13094         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13095         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13096 
13097         // Force IllegalArgumentException
13098         calibrator = null;
13099         try {
13100             calibrator = new KnownGravityNormAccelerometerCalibrator(
13101                     -gravityNorm, measurements,
13102                     true, biasX, biasY, biasZ, sx, sy, sz,
13103                     this);
13104             fail("IllegalArgumentException expected but not thrown");
13105         } catch (final IllegalArgumentException ignore) {
13106         }
13107         assertNull(calibrator);
13108     }
13109 
13110     @Test
13111     public void testConstructor110() throws WrongSizeException {
13112         final Matrix ba = generateBa();
13113         final double biasX = ba.getElementAtIndex(0);
13114         final double biasY = ba.getElementAtIndex(1);
13115         final double biasZ = ba.getElementAtIndex(2);
13116 
13117         final Acceleration bx = new Acceleration(biasX,
13118                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13119         final Acceleration by = new Acceleration(biasY,
13120                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13121         final Acceleration bz = new Acceleration(biasZ,
13122                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13123 
13124         final Matrix ma = generateMaCommonAxis();
13125         final double sx = ma.getElementAt(0, 0);
13126         final double sy = ma.getElementAt(1, 1);
13127         final double sz = ma.getElementAt(2, 2);
13128 
13129         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13130         final double latitude = Math.toRadians(
13131                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13132         final double longitude = Math.toRadians(
13133                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13134         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13135         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13136         final NEDVelocity nedVelocity = new NEDVelocity();
13137         final ECEFPosition ecefPosition = new ECEFPosition();
13138         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13139         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13140                 ecefPosition, ecefVelocity);
13141         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13142                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13143         final double gravityNorm = gravity.getNorm();
13144 
13145         KnownGravityNormAccelerometerCalibrator calibrator =
13146                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
13147                         bx, by, bz, sx, sy, sz);
13148 
13149         // check default values
13150         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13151         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13152         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13153         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13154         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13155         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13156         final Acceleration bx2 = new Acceleration(0.0,
13157                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13158         calibrator.getInitialBiasXAsAcceleration(bx2);
13159         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13160         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13161         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13162         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13163         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13164         final Acceleration by2 = new Acceleration(0.0,
13165                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13166         calibrator.getInitialBiasYAsAcceleration(by2);
13167         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13168         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13169         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13170         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13171         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13172         final Acceleration bz2 = new Acceleration(0.0,
13173                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13174         calibrator.getInitialBiasZAsAcceleration(bz2);
13175         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13176         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13177         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13178         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13179         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13180         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13181         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13182         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13183         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13184         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13185         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13186         final double[] bias1 = calibrator.getInitialBias();
13187         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13188         final double[] bias2 = new double[3];
13189         calibrator.getInitialBias(bias2);
13190         assertArrayEquals(bias1, bias2, 0.0);
13191         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13192         assertEquals(b1, ba);
13193         final Matrix b2 = new Matrix(3, 1);
13194         calibrator.getInitialBiasAsMatrix(b2);
13195         assertEquals(b1, b2);
13196         final Matrix ma1 = calibrator.getInitialMa();
13197         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13198         final Matrix ma2 = new Matrix(3, 3);
13199         calibrator.getInitialMa(ma2);
13200         assertEquals(ma1, ma2);
13201         assertNull(calibrator.getMeasurements());
13202         assertFalse(calibrator.isCommonAxisUsed());
13203         assertNull(calibrator.getListener());
13204         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13205         assertFalse(calibrator.isReady());
13206         assertFalse(calibrator.isRunning());
13207         assertNull(calibrator.getEstimatedBiases());
13208         assertFalse(calibrator.getEstimatedBiases(null));
13209         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13210         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13211         assertNull(calibrator.getEstimatedBiasFx());
13212         assertNull(calibrator.getEstimatedBiasFy());
13213         assertNull(calibrator.getEstimatedBiasFz());
13214         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13215         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13216         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13217         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13218         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13219         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13220         assertNull(calibrator.getEstimatedMa());
13221         assertNull(calibrator.getEstimatedSx());
13222         assertNull(calibrator.getEstimatedSy());
13223         assertNull(calibrator.getEstimatedSz());
13224         assertNull(calibrator.getEstimatedMxy());
13225         assertNull(calibrator.getEstimatedMxz());
13226         assertNull(calibrator.getEstimatedMyx());
13227         assertNull(calibrator.getEstimatedMyz());
13228         assertNull(calibrator.getEstimatedMzx());
13229         assertNull(calibrator.getEstimatedMzy());
13230         assertNull(calibrator.getEstimatedCovariance());
13231         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13232         assertNotNull(calibrator.getGroundTruthGravityNorm());
13233         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13234         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13235         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13236                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13237         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13238         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13239         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13240 
13241         // Force IllegalArgumentException
13242         calibrator = null;
13243         try {
13244             calibrator = new KnownGravityNormAccelerometerCalibrator(
13245                     -gravityNorm, bx, by, bz, sx, sy, sz);
13246             fail("IllegalArgumentException expected but not thrown");
13247         } catch (final IllegalArgumentException ignore) {
13248         }
13249         assertNull(calibrator);
13250     }
13251 
13252     @Test
13253     public void testConstructor111() throws WrongSizeException {
13254         final Matrix ba = generateBa();
13255         final double biasX = ba.getElementAtIndex(0);
13256         final double biasY = ba.getElementAtIndex(1);
13257         final double biasZ = ba.getElementAtIndex(2);
13258 
13259         final Acceleration bx = new Acceleration(biasX,
13260                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13261         final Acceleration by = new Acceleration(biasY,
13262                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13263         final Acceleration bz = new Acceleration(biasZ,
13264                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13265 
13266         final Matrix ma = generateMaCommonAxis();
13267         final double sx = ma.getElementAt(0, 0);
13268         final double sy = ma.getElementAt(1, 1);
13269         final double sz = ma.getElementAt(2, 2);
13270 
13271         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13272         final double latitude = Math.toRadians(
13273                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13274         final double longitude = Math.toRadians(
13275                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13276         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13277         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13278         final NEDVelocity nedVelocity = new NEDVelocity();
13279         final ECEFPosition ecefPosition = new ECEFPosition();
13280         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13281         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13282                 ecefPosition, ecefVelocity);
13283         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13284                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13285         final double gravityNorm = gravity.getNorm();
13286 
13287         KnownGravityNormAccelerometerCalibrator calibrator =
13288                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
13289                         bx, by, bz, sx, sy, sz, this);
13290 
13291         // check default values
13292         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13293         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13294         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13295         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13296         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13297         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13298         final Acceleration bx2 = new Acceleration(0.0,
13299                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13300         calibrator.getInitialBiasXAsAcceleration(bx2);
13301         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13302         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13303         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13304         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13305         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13306         final Acceleration by2 = new Acceleration(0.0,
13307                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13308         calibrator.getInitialBiasYAsAcceleration(by2);
13309         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13310         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13311         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13312         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13313         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13314         final Acceleration bz2 = new Acceleration(0.0,
13315                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13316         calibrator.getInitialBiasZAsAcceleration(bz2);
13317         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13318         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13319         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13320         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13321         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13322         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13323         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13324         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13325         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13326         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13327         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13328         final double[] bias1 = calibrator.getInitialBias();
13329         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13330         final double[] bias2 = new double[3];
13331         calibrator.getInitialBias(bias2);
13332         assertArrayEquals(bias1, bias2, 0.0);
13333         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13334         assertEquals(b1, ba);
13335         final Matrix b2 = new Matrix(3, 1);
13336         calibrator.getInitialBiasAsMatrix(b2);
13337         assertEquals(b1, b2);
13338         final Matrix ma1 = calibrator.getInitialMa();
13339         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13340         final Matrix ma2 = new Matrix(3, 3);
13341         calibrator.getInitialMa(ma2);
13342         assertEquals(ma1, ma2);
13343         assertNull(calibrator.getMeasurements());
13344         assertFalse(calibrator.isCommonAxisUsed());
13345         assertSame(calibrator.getListener(), this);
13346         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13347         assertFalse(calibrator.isReady());
13348         assertFalse(calibrator.isRunning());
13349         assertNull(calibrator.getEstimatedBiases());
13350         assertFalse(calibrator.getEstimatedBiases(null));
13351         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13352         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13353         assertNull(calibrator.getEstimatedBiasFx());
13354         assertNull(calibrator.getEstimatedBiasFy());
13355         assertNull(calibrator.getEstimatedBiasFz());
13356         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13357         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13358         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13359         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13360         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13361         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13362         assertNull(calibrator.getEstimatedMa());
13363         assertNull(calibrator.getEstimatedSx());
13364         assertNull(calibrator.getEstimatedSy());
13365         assertNull(calibrator.getEstimatedSz());
13366         assertNull(calibrator.getEstimatedMxy());
13367         assertNull(calibrator.getEstimatedMxz());
13368         assertNull(calibrator.getEstimatedMyx());
13369         assertNull(calibrator.getEstimatedMyz());
13370         assertNull(calibrator.getEstimatedMzx());
13371         assertNull(calibrator.getEstimatedMzy());
13372         assertNull(calibrator.getEstimatedCovariance());
13373         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13374         assertNotNull(calibrator.getGroundTruthGravityNorm());
13375         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13376         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13377         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13378                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13379         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13380         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13381         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13382 
13383         // Force IllegalArgumentException
13384         calibrator = null;
13385         try {
13386             calibrator = new KnownGravityNormAccelerometerCalibrator(
13387                     -gravityNorm, bx, by, bz, sx, sy, sz, this);
13388             fail("IllegalArgumentException expected but not thrown");
13389         } catch (final IllegalArgumentException ignore) {
13390         }
13391         assertNull(calibrator);
13392     }
13393 
13394     @Test
13395     public void testConstructor112() throws WrongSizeException {
13396         final Collection<StandardDeviationBodyKinematics> measurements =
13397                 Collections.emptyList();
13398 
13399         final Matrix ba = generateBa();
13400         final double biasX = ba.getElementAtIndex(0);
13401         final double biasY = ba.getElementAtIndex(1);
13402         final double biasZ = ba.getElementAtIndex(2);
13403 
13404         final Acceleration bx = new Acceleration(biasX,
13405                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13406         final Acceleration by = new Acceleration(biasY,
13407                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13408         final Acceleration bz = new Acceleration(biasZ,
13409                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13410 
13411         final Matrix ma = generateMaCommonAxis();
13412         final double sx = ma.getElementAt(0, 0);
13413         final double sy = ma.getElementAt(1, 1);
13414         final double sz = ma.getElementAt(2, 2);
13415 
13416         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13417         final double latitude = Math.toRadians(
13418                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13419         final double longitude = Math.toRadians(
13420                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13421         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13422         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13423         final NEDVelocity nedVelocity = new NEDVelocity();
13424         final ECEFPosition ecefPosition = new ECEFPosition();
13425         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13426         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13427                 ecefPosition, ecefVelocity);
13428         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13429                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13430         final double gravityNorm = gravity.getNorm();
13431 
13432         KnownGravityNormAccelerometerCalibrator calibrator =
13433                 new KnownGravityNormAccelerometerCalibrator(
13434                         gravityNorm, measurements,
13435                         bx, by, bz, sx, sy, sz);
13436 
13437         // check default values
13438         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13439         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13440         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13441         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13442         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13443         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13444         final Acceleration bx2 = new Acceleration(0.0,
13445                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13446         calibrator.getInitialBiasXAsAcceleration(bx2);
13447         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13448         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13449         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13450         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13451         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13452         final Acceleration by2 = new Acceleration(0.0,
13453                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13454         calibrator.getInitialBiasYAsAcceleration(by2);
13455         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13456         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13457         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13458         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13459         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13460         final Acceleration bz2 = new Acceleration(0.0,
13461                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13462         calibrator.getInitialBiasZAsAcceleration(bz2);
13463         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13464         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13465         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13466         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13467         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13468         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13469         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13470         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13471         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13472         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13473         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13474         final double[] bias1 = calibrator.getInitialBias();
13475         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13476         final double[] bias2 = new double[3];
13477         calibrator.getInitialBias(bias2);
13478         assertArrayEquals(bias1, bias2, 0.0);
13479         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13480         assertEquals(b1, ba);
13481         final Matrix b2 = new Matrix(3, 1);
13482         calibrator.getInitialBiasAsMatrix(b2);
13483         assertEquals(b1, b2);
13484         final Matrix ma1 = calibrator.getInitialMa();
13485         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13486         final Matrix ma2 = new Matrix(3, 3);
13487         calibrator.getInitialMa(ma2);
13488         assertEquals(ma1, ma2);
13489         assertSame(calibrator.getMeasurements(), measurements);
13490         assertFalse(calibrator.isCommonAxisUsed());
13491         assertNull(calibrator.getListener());
13492         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13493         assertFalse(calibrator.isReady());
13494         assertFalse(calibrator.isRunning());
13495         assertNull(calibrator.getEstimatedBiases());
13496         assertFalse(calibrator.getEstimatedBiases(null));
13497         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13498         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13499         assertNull(calibrator.getEstimatedBiasFx());
13500         assertNull(calibrator.getEstimatedBiasFy());
13501         assertNull(calibrator.getEstimatedBiasFz());
13502         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13503         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13504         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13505         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13506         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13507         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13508         assertNull(calibrator.getEstimatedMa());
13509         assertNull(calibrator.getEstimatedSx());
13510         assertNull(calibrator.getEstimatedSy());
13511         assertNull(calibrator.getEstimatedSz());
13512         assertNull(calibrator.getEstimatedMxy());
13513         assertNull(calibrator.getEstimatedMxz());
13514         assertNull(calibrator.getEstimatedMyx());
13515         assertNull(calibrator.getEstimatedMyz());
13516         assertNull(calibrator.getEstimatedMzx());
13517         assertNull(calibrator.getEstimatedMzy());
13518         assertNull(calibrator.getEstimatedCovariance());
13519         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13520         assertNotNull(calibrator.getGroundTruthGravityNorm());
13521         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13522         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13523         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13524                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13525         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13526         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13527         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13528 
13529         // Force IllegalArgumentException
13530         calibrator = null;
13531         try {
13532             calibrator = new KnownGravityNormAccelerometerCalibrator(
13533                     -gravityNorm, measurements,
13534                     bx, by, bz, sx, sy, sz);
13535             fail("IllegalArgumentException expected but not thrown");
13536         } catch (final IllegalArgumentException ignore) {
13537         }
13538         assertNull(calibrator);
13539     }
13540 
13541     @Test
13542     public void testConstructor113() throws WrongSizeException {
13543         final Collection<StandardDeviationBodyKinematics> measurements =
13544                 Collections.emptyList();
13545 
13546         final Matrix ba = generateBa();
13547         final double biasX = ba.getElementAtIndex(0);
13548         final double biasY = ba.getElementAtIndex(1);
13549         final double biasZ = ba.getElementAtIndex(2);
13550 
13551         final Acceleration bx = new Acceleration(biasX,
13552                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13553         final Acceleration by = new Acceleration(biasY,
13554                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13555         final Acceleration bz = new Acceleration(biasZ,
13556                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13557 
13558         final Matrix ma = generateMaCommonAxis();
13559         final double sx = ma.getElementAt(0, 0);
13560         final double sy = ma.getElementAt(1, 1);
13561         final double sz = ma.getElementAt(2, 2);
13562 
13563         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13564         final double latitude = Math.toRadians(
13565                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13566         final double longitude = Math.toRadians(
13567                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13568         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13569         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13570         final NEDVelocity nedVelocity = new NEDVelocity();
13571         final ECEFPosition ecefPosition = new ECEFPosition();
13572         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13573         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13574                 ecefPosition, ecefVelocity);
13575         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13576                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13577         final double gravityNorm = gravity.getNorm();
13578 
13579         KnownGravityNormAccelerometerCalibrator calibrator =
13580                 new KnownGravityNormAccelerometerCalibrator(
13581                         gravityNorm, measurements,
13582                         bx, by, bz, sx, sy, sz, this);
13583 
13584         // check default values
13585         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13586         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13587         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13588         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13589         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13590         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13591         final Acceleration bx2 = new Acceleration(0.0,
13592                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13593         calibrator.getInitialBiasXAsAcceleration(bx2);
13594         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13595         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13596         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13597         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13598         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13599         final Acceleration by2 = new Acceleration(0.0,
13600                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13601         calibrator.getInitialBiasYAsAcceleration(by2);
13602         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13603         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13604         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13605         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13606         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13607         final Acceleration bz2 = new Acceleration(0.0,
13608                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13609         calibrator.getInitialBiasZAsAcceleration(bz2);
13610         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13611         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13612         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13613         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13614         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13615         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13616         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13617         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13618         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13619         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13620         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13621         final double[] bias1 = calibrator.getInitialBias();
13622         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13623         final double[] bias2 = new double[3];
13624         calibrator.getInitialBias(bias2);
13625         assertArrayEquals(bias1, bias2, 0.0);
13626         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13627         assertEquals(b1, ba);
13628         final Matrix b2 = new Matrix(3, 1);
13629         calibrator.getInitialBiasAsMatrix(b2);
13630         assertEquals(b1, b2);
13631         final Matrix ma1 = calibrator.getInitialMa();
13632         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13633         final Matrix ma2 = new Matrix(3, 3);
13634         calibrator.getInitialMa(ma2);
13635         assertEquals(ma1, ma2);
13636         assertSame(calibrator.getMeasurements(), measurements);
13637         assertFalse(calibrator.isCommonAxisUsed());
13638         assertSame(calibrator.getListener(), this);
13639         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13640         assertFalse(calibrator.isReady());
13641         assertFalse(calibrator.isRunning());
13642         assertNull(calibrator.getEstimatedBiases());
13643         assertFalse(calibrator.getEstimatedBiases(null));
13644         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13645         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13646         assertNull(calibrator.getEstimatedBiasFx());
13647         assertNull(calibrator.getEstimatedBiasFy());
13648         assertNull(calibrator.getEstimatedBiasFz());
13649         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13650         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13651         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13652         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13653         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13654         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13655         assertNull(calibrator.getEstimatedMa());
13656         assertNull(calibrator.getEstimatedSx());
13657         assertNull(calibrator.getEstimatedSy());
13658         assertNull(calibrator.getEstimatedSz());
13659         assertNull(calibrator.getEstimatedMxy());
13660         assertNull(calibrator.getEstimatedMxz());
13661         assertNull(calibrator.getEstimatedMyx());
13662         assertNull(calibrator.getEstimatedMyz());
13663         assertNull(calibrator.getEstimatedMzx());
13664         assertNull(calibrator.getEstimatedMzy());
13665         assertNull(calibrator.getEstimatedCovariance());
13666         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13667         assertNotNull(calibrator.getGroundTruthGravityNorm());
13668         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13669         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13670         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13671                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13672         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13673         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13674         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13675 
13676         // Force IllegalArgumentException
13677         calibrator = null;
13678         try {
13679             calibrator = new KnownGravityNormAccelerometerCalibrator(
13680                     -gravityNorm, measurements,
13681                     bx, by, bz, sx, sy, sz, this);
13682             fail("IllegalArgumentException expected but not thrown");
13683         } catch (final IllegalArgumentException ignore) {
13684         }
13685         assertNull(calibrator);
13686     }
13687 
13688     @Test
13689     public void testConstructor114() throws WrongSizeException {
13690         final Matrix ba = generateBa();
13691         final double biasX = ba.getElementAtIndex(0);
13692         final double biasY = ba.getElementAtIndex(1);
13693         final double biasZ = ba.getElementAtIndex(2);
13694 
13695         final Acceleration bx = new Acceleration(biasX,
13696                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13697         final Acceleration by = new Acceleration(biasY,
13698                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13699         final Acceleration bz = new Acceleration(biasZ,
13700                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13701 
13702         final Matrix ma = generateMaCommonAxis();
13703         final double sx = ma.getElementAt(0, 0);
13704         final double sy = ma.getElementAt(1, 1);
13705         final double sz = ma.getElementAt(2, 2);
13706 
13707         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13708         final double latitude = Math.toRadians(
13709                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13710         final double longitude = Math.toRadians(
13711                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13712         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13713         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13714         final NEDVelocity nedVelocity = new NEDVelocity();
13715         final ECEFPosition ecefPosition = new ECEFPosition();
13716         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13717         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13718                 ecefPosition, ecefVelocity);
13719         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13720                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13721         final double gravityNorm = gravity.getNorm();
13722 
13723         KnownGravityNormAccelerometerCalibrator calibrator =
13724                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
13725                         true, bx, by, bz, sx, sy, sz);
13726 
13727         // check default values
13728         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13729         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13730         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13731         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13732         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13733         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13734         final Acceleration bx2 = new Acceleration(0.0,
13735                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13736         calibrator.getInitialBiasXAsAcceleration(bx2);
13737         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13738         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13739         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13740         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13741         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13742         final Acceleration by2 = new Acceleration(0.0,
13743                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13744         calibrator.getInitialBiasYAsAcceleration(by2);
13745         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13746         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13747         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13748         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13749         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13750         final Acceleration bz2 = new Acceleration(0.0,
13751                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13752         calibrator.getInitialBiasZAsAcceleration(bz2);
13753         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13754         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13755         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13756         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13757         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13758         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13759         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13760         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13761         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13762         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13763         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13764         final double[] bias1 = calibrator.getInitialBias();
13765         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13766         final double[] bias2 = new double[3];
13767         calibrator.getInitialBias(bias2);
13768         assertArrayEquals(bias1, bias2, 0.0);
13769         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13770         assertEquals(b1, ba);
13771         final Matrix b2 = new Matrix(3, 1);
13772         calibrator.getInitialBiasAsMatrix(b2);
13773         assertEquals(b1, b2);
13774         final Matrix ma1 = calibrator.getInitialMa();
13775         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13776         final Matrix ma2 = new Matrix(3, 3);
13777         calibrator.getInitialMa(ma2);
13778         assertEquals(ma1, ma2);
13779         assertNull(calibrator.getMeasurements());
13780         assertTrue(calibrator.isCommonAxisUsed());
13781         assertNull(calibrator.getListener());
13782         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13783         assertFalse(calibrator.isReady());
13784         assertFalse(calibrator.isRunning());
13785         assertNull(calibrator.getEstimatedBiases());
13786         assertFalse(calibrator.getEstimatedBiases(null));
13787         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13788         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13789         assertNull(calibrator.getEstimatedBiasFx());
13790         assertNull(calibrator.getEstimatedBiasFy());
13791         assertNull(calibrator.getEstimatedBiasFz());
13792         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13793         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13794         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13795         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13796         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13797         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13798         assertNull(calibrator.getEstimatedMa());
13799         assertNull(calibrator.getEstimatedSx());
13800         assertNull(calibrator.getEstimatedSy());
13801         assertNull(calibrator.getEstimatedSz());
13802         assertNull(calibrator.getEstimatedMxy());
13803         assertNull(calibrator.getEstimatedMxz());
13804         assertNull(calibrator.getEstimatedMyx());
13805         assertNull(calibrator.getEstimatedMyz());
13806         assertNull(calibrator.getEstimatedMzx());
13807         assertNull(calibrator.getEstimatedMzy());
13808         assertNull(calibrator.getEstimatedCovariance());
13809         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13810         assertNotNull(calibrator.getGroundTruthGravityNorm());
13811         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13812         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13813         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13814                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13815         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13816         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13817         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13818 
13819         // Force IllegalArgumentException
13820         calibrator = null;
13821         try {
13822             calibrator = new KnownGravityNormAccelerometerCalibrator(
13823                     -gravityNorm, true, bx, by, bz,
13824                     sx, sy, sz);
13825             fail("IllegalArgumentException expected but not thrown");
13826         } catch (final IllegalArgumentException ignore) {
13827         }
13828         assertNull(calibrator);
13829     }
13830 
13831     @Test
13832     public void testConstructor115() throws WrongSizeException {
13833         final Matrix ba = generateBa();
13834         final double biasX = ba.getElementAtIndex(0);
13835         final double biasY = ba.getElementAtIndex(1);
13836         final double biasZ = ba.getElementAtIndex(2);
13837 
13838         final Acceleration bx = new Acceleration(biasX,
13839                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13840         final Acceleration by = new Acceleration(biasY,
13841                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13842         final Acceleration bz = new Acceleration(biasZ,
13843                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13844 
13845         final Matrix ma = generateMaCommonAxis();
13846         final double sx = ma.getElementAt(0, 0);
13847         final double sy = ma.getElementAt(1, 1);
13848         final double sz = ma.getElementAt(2, 2);
13849 
13850         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13851         final double latitude = Math.toRadians(
13852                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13853         final double longitude = Math.toRadians(
13854                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13855         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13856         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13857         final NEDVelocity nedVelocity = new NEDVelocity();
13858         final ECEFPosition ecefPosition = new ECEFPosition();
13859         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13860         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13861                 ecefPosition, ecefVelocity);
13862         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13863                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13864         final double gravityNorm = gravity.getNorm();
13865 
13866         KnownGravityNormAccelerometerCalibrator calibrator =
13867                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
13868                         true, bx, by, bz, sx, sy, sz,
13869                         this);
13870 
13871         // check default values
13872         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13873         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13874         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13875         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13876         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13877         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13878         final Acceleration bx2 = new Acceleration(0.0,
13879                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13880         calibrator.getInitialBiasXAsAcceleration(bx2);
13881         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13882         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13883         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13884         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13885         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13886         final Acceleration by2 = new Acceleration(0.0,
13887                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13888         calibrator.getInitialBiasYAsAcceleration(by2);
13889         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13890         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13891         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13892         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13893         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13894         final Acceleration bz2 = new Acceleration(0.0,
13895                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13896         calibrator.getInitialBiasZAsAcceleration(bz2);
13897         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13898         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13899         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13900         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13901         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13902         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13903         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13904         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13905         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13906         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13907         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13908         final double[] bias1 = calibrator.getInitialBias();
13909         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13910         final double[] bias2 = new double[3];
13911         calibrator.getInitialBias(bias2);
13912         assertArrayEquals(bias1, bias2, 0.0);
13913         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13914         assertEquals(b1, ba);
13915         final Matrix b2 = new Matrix(3, 1);
13916         calibrator.getInitialBiasAsMatrix(b2);
13917         assertEquals(b1, b2);
13918         final Matrix ma1 = calibrator.getInitialMa();
13919         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13920         final Matrix ma2 = new Matrix(3, 3);
13921         calibrator.getInitialMa(ma2);
13922         assertEquals(ma1, ma2);
13923         assertNull(calibrator.getMeasurements());
13924         assertTrue(calibrator.isCommonAxisUsed());
13925         assertSame(calibrator.getListener(), this);
13926         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13927         assertFalse(calibrator.isReady());
13928         assertFalse(calibrator.isRunning());
13929         assertNull(calibrator.getEstimatedBiases());
13930         assertFalse(calibrator.getEstimatedBiases(null));
13931         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13932         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13933         assertNull(calibrator.getEstimatedBiasFx());
13934         assertNull(calibrator.getEstimatedBiasFy());
13935         assertNull(calibrator.getEstimatedBiasFz());
13936         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13937         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13938         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13939         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13940         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13941         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13942         assertNull(calibrator.getEstimatedMa());
13943         assertNull(calibrator.getEstimatedSx());
13944         assertNull(calibrator.getEstimatedSy());
13945         assertNull(calibrator.getEstimatedSz());
13946         assertNull(calibrator.getEstimatedMxy());
13947         assertNull(calibrator.getEstimatedMxz());
13948         assertNull(calibrator.getEstimatedMyx());
13949         assertNull(calibrator.getEstimatedMyz());
13950         assertNull(calibrator.getEstimatedMzx());
13951         assertNull(calibrator.getEstimatedMzy());
13952         assertNull(calibrator.getEstimatedCovariance());
13953         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13954         assertNotNull(calibrator.getGroundTruthGravityNorm());
13955         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13956         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13957         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13958                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13959         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13960         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13961         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13962 
13963         // Force IllegalArgumentException
13964         calibrator = null;
13965         try {
13966             calibrator = new KnownGravityNormAccelerometerCalibrator(
13967                     -gravityNorm, true, bx, by, bz, sx, sy, sz,
13968                     this);
13969             fail("IllegalArgumentException expected but not thrown");
13970         } catch (final IllegalArgumentException ignore) {
13971         }
13972         assertNull(calibrator);
13973     }
13974 
13975     @Test
13976     public void testConstructor116() throws WrongSizeException {
13977         final Collection<StandardDeviationBodyKinematics> measurements =
13978                 Collections.emptyList();
13979 
13980         final Matrix ba = generateBa();
13981         final double biasX = ba.getElementAtIndex(0);
13982         final double biasY = ba.getElementAtIndex(1);
13983         final double biasZ = ba.getElementAtIndex(2);
13984 
13985         final Acceleration bx = new Acceleration(biasX,
13986                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13987         final Acceleration by = new Acceleration(biasY,
13988                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13989         final Acceleration bz = new Acceleration(biasZ,
13990                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13991 
13992         final Matrix ma = generateMaCommonAxis();
13993         final double sx = ma.getElementAt(0, 0);
13994         final double sy = ma.getElementAt(1, 1);
13995         final double sz = ma.getElementAt(2, 2);
13996 
13997         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13998         final double latitude = Math.toRadians(
13999                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14000         final double longitude = Math.toRadians(
14001                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14002         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14003         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14004         final NEDVelocity nedVelocity = new NEDVelocity();
14005         final ECEFPosition ecefPosition = new ECEFPosition();
14006         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14007         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14008                 ecefPosition, ecefVelocity);
14009         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14010                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14011         final double gravityNorm = gravity.getNorm();
14012 
14013         KnownGravityNormAccelerometerCalibrator calibrator =
14014                 new KnownGravityNormAccelerometerCalibrator(
14015                         gravityNorm, measurements,
14016                         true, bx, by, bz, sx, sy, sz);
14017 
14018         // check default values
14019         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14020         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14021         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14022         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14023         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14024         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14025         final Acceleration bx2 = new Acceleration(0.0,
14026                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14027         calibrator.getInitialBiasXAsAcceleration(bx2);
14028         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14029         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14030         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14031         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14032         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14033         final Acceleration by2 = new Acceleration(0.0,
14034                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14035         calibrator.getInitialBiasYAsAcceleration(by2);
14036         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14037         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14038         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14039         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14040         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14041         final Acceleration bz2 = new Acceleration(0.0,
14042                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14043         calibrator.getInitialBiasZAsAcceleration(bz2);
14044         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14045         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14046         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14047         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14048         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14049         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
14050         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
14051         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
14052         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
14053         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
14054         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
14055         final double[] bias1 = calibrator.getInitialBias();
14056         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14057         final double[] bias2 = new double[3];
14058         calibrator.getInitialBias(bias2);
14059         assertArrayEquals(bias1, bias2, 0.0);
14060         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14061         assertEquals(b1, ba);
14062         final Matrix b2 = new Matrix(3, 1);
14063         calibrator.getInitialBiasAsMatrix(b2);
14064         assertEquals(b1, b2);
14065         final Matrix ma1 = calibrator.getInitialMa();
14066         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
14067         final Matrix ma2 = new Matrix(3, 3);
14068         calibrator.getInitialMa(ma2);
14069         assertEquals(ma1, ma2);
14070         assertSame(calibrator.getMeasurements(), measurements);
14071         assertTrue(calibrator.isCommonAxisUsed());
14072         assertNull(calibrator.getListener());
14073         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14074         assertFalse(calibrator.isReady());
14075         assertFalse(calibrator.isRunning());
14076         assertNull(calibrator.getEstimatedBiases());
14077         assertFalse(calibrator.getEstimatedBiases(null));
14078         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14079         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14080         assertNull(calibrator.getEstimatedBiasFx());
14081         assertNull(calibrator.getEstimatedBiasFy());
14082         assertNull(calibrator.getEstimatedBiasFz());
14083         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14084         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14085         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14086         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14087         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14088         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14089         assertNull(calibrator.getEstimatedMa());
14090         assertNull(calibrator.getEstimatedSx());
14091         assertNull(calibrator.getEstimatedSy());
14092         assertNull(calibrator.getEstimatedSz());
14093         assertNull(calibrator.getEstimatedMxy());
14094         assertNull(calibrator.getEstimatedMxz());
14095         assertNull(calibrator.getEstimatedMyx());
14096         assertNull(calibrator.getEstimatedMyz());
14097         assertNull(calibrator.getEstimatedMzx());
14098         assertNull(calibrator.getEstimatedMzy());
14099         assertNull(calibrator.getEstimatedCovariance());
14100         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14101         assertNotNull(calibrator.getGroundTruthGravityNorm());
14102         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14103         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14104         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14105                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14106         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14107         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14108         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14109 
14110         // Force IllegalArgumentException
14111         calibrator = null;
14112         try {
14113             calibrator = new KnownGravityNormAccelerometerCalibrator(
14114                     -gravityNorm, measurements,
14115                     true, bx, by, bz, sx, sy, sz);
14116             fail("IllegalArgumentException expected but not thrown");
14117         } catch (final IllegalArgumentException ignore) {
14118         }
14119         assertNull(calibrator);
14120     }
14121 
14122     @Test
14123     public void testConstructor117() throws WrongSizeException {
14124         final Collection<StandardDeviationBodyKinematics> measurements =
14125                 Collections.emptyList();
14126 
14127         final Matrix ba = generateBa();
14128         final double biasX = ba.getElementAtIndex(0);
14129         final double biasY = ba.getElementAtIndex(1);
14130         final double biasZ = ba.getElementAtIndex(2);
14131 
14132         final Acceleration bx = new Acceleration(biasX,
14133                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14134         final Acceleration by = new Acceleration(biasY,
14135                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14136         final Acceleration bz = new Acceleration(biasZ,
14137                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14138 
14139         final Matrix ma = generateMaCommonAxis();
14140         final double sx = ma.getElementAt(0, 0);
14141         final double sy = ma.getElementAt(1, 1);
14142         final double sz = ma.getElementAt(2, 2);
14143 
14144         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14145         final double latitude = Math.toRadians(
14146                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14147         final double longitude = Math.toRadians(
14148                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14149         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14150         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14151         final NEDVelocity nedVelocity = new NEDVelocity();
14152         final ECEFPosition ecefPosition = new ECEFPosition();
14153         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14154         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14155                 ecefPosition, ecefVelocity);
14156         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14157                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14158         final double gravityNorm = gravity.getNorm();
14159 
14160         KnownGravityNormAccelerometerCalibrator calibrator =
14161                 new KnownGravityNormAccelerometerCalibrator(
14162                         gravityNorm, measurements,
14163                         true, bx, by, bz, sx, sy, sz, this);
14164 
14165         // check default values
14166         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14167         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14168         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14169         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14170         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14171         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14172         final Acceleration bx2 = new Acceleration(0.0,
14173                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14174         calibrator.getInitialBiasXAsAcceleration(bx2);
14175         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14176         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14177         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14178         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14179         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14180         final Acceleration by2 = new Acceleration(0.0,
14181                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14182         calibrator.getInitialBiasYAsAcceleration(by2);
14183         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14184         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14185         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14186         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14187         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14188         final Acceleration bz2 = new Acceleration(0.0,
14189                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14190         calibrator.getInitialBiasZAsAcceleration(bz2);
14191         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14192         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14193         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14194         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14195         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14196         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
14197         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
14198         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
14199         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
14200         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
14201         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
14202         final double[] bias1 = calibrator.getInitialBias();
14203         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14204         final double[] bias2 = new double[3];
14205         calibrator.getInitialBias(bias2);
14206         assertArrayEquals(bias1, bias2, 0.0);
14207         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14208         assertEquals(b1, ba);
14209         final Matrix b2 = new Matrix(3, 1);
14210         calibrator.getInitialBiasAsMatrix(b2);
14211         assertEquals(b1, b2);
14212         final Matrix ma1 = calibrator.getInitialMa();
14213         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
14214         final Matrix ma2 = new Matrix(3, 3);
14215         calibrator.getInitialMa(ma2);
14216         assertEquals(ma1, ma2);
14217         assertSame(calibrator.getMeasurements(), measurements);
14218         assertTrue(calibrator.isCommonAxisUsed());
14219         assertSame(calibrator.getListener(), this);
14220         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14221         assertFalse(calibrator.isReady());
14222         assertFalse(calibrator.isRunning());
14223         assertNull(calibrator.getEstimatedBiases());
14224         assertFalse(calibrator.getEstimatedBiases(null));
14225         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14226         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14227         assertNull(calibrator.getEstimatedBiasFx());
14228         assertNull(calibrator.getEstimatedBiasFy());
14229         assertNull(calibrator.getEstimatedBiasFz());
14230         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14231         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14232         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14233         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14234         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14235         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14236         assertNull(calibrator.getEstimatedMa());
14237         assertNull(calibrator.getEstimatedSx());
14238         assertNull(calibrator.getEstimatedSy());
14239         assertNull(calibrator.getEstimatedSz());
14240         assertNull(calibrator.getEstimatedMxy());
14241         assertNull(calibrator.getEstimatedMxz());
14242         assertNull(calibrator.getEstimatedMyx());
14243         assertNull(calibrator.getEstimatedMyz());
14244         assertNull(calibrator.getEstimatedMzx());
14245         assertNull(calibrator.getEstimatedMzy());
14246         assertNull(calibrator.getEstimatedCovariance());
14247         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14248         assertNotNull(calibrator.getGroundTruthGravityNorm());
14249         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14250         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14251         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14252                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14253         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14254         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14255         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14256 
14257         // Force IllegalArgumentException
14258         calibrator = null;
14259         try {
14260             calibrator = new KnownGravityNormAccelerometerCalibrator(
14261                     -gravityNorm, measurements,
14262                     true, bx, by, bz, sx, sy, sz, this);
14263             fail("IllegalArgumentException expected but not thrown");
14264         } catch (final IllegalArgumentException ignore) {
14265         }
14266         assertNull(calibrator);
14267     }
14268 
14269     @Test
14270     public void testConstructor118() throws WrongSizeException {
14271         final Matrix ba = generateBa();
14272         final double biasX = ba.getElementAtIndex(0);
14273         final double biasY = ba.getElementAtIndex(1);
14274         final double biasZ = ba.getElementAtIndex(2);
14275 
14276         final Matrix ma = generateMaCommonAxis();
14277         final double sx = ma.getElementAt(0, 0);
14278         final double sy = ma.getElementAt(1, 1);
14279         final double sz = ma.getElementAt(2, 2);
14280         final double mxy = ma.getElementAt(0, 1);
14281         final double mxz = ma.getElementAt(0, 2);
14282         final double myx = ma.getElementAt(1, 0);
14283         final double myz = ma.getElementAt(1, 2);
14284         final double mzx = ma.getElementAt(2, 0);
14285         final double mzy = ma.getElementAt(2, 1);
14286 
14287         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14288         final double latitude = Math.toRadians(
14289                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14290         final double longitude = Math.toRadians(
14291                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14292         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14293         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14294         final NEDVelocity nedVelocity = new NEDVelocity();
14295         final ECEFPosition ecefPosition = new ECEFPosition();
14296         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14297         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14298                 ecefPosition, ecefVelocity);
14299         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14300                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14301         final double gravityNorm = gravity.getNorm();
14302 
14303         KnownGravityNormAccelerometerCalibrator calibrator =
14304                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
14305                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14306                         myx, myz, mzx, mzy);
14307 
14308         // check default values
14309         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14310         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14311         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14312         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14313         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14314         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14315         final Acceleration bx2 = new Acceleration(0.0,
14316                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14317         calibrator.getInitialBiasXAsAcceleration(bx2);
14318         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14319         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14320         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14321         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14322         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14323         final Acceleration by2 = new Acceleration(0.0,
14324                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14325         calibrator.getInitialBiasYAsAcceleration(by2);
14326         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14327         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14328         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14329         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14330         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14331         final Acceleration bz2 = new Acceleration(0.0,
14332                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14333         calibrator.getInitialBiasZAsAcceleration(bz2);
14334         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14335         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14336         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14337         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14338         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14339         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14340         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14341         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14342         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14343         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14344         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14345         final double[] bias1 = calibrator.getInitialBias();
14346         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14347         final double[] bias2 = new double[3];
14348         calibrator.getInitialBias(bias2);
14349         assertArrayEquals(bias1, bias2, 0.0);
14350         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14351         assertEquals(b1, ba);
14352         final Matrix b2 = new Matrix(3, 1);
14353         calibrator.getInitialBiasAsMatrix(b2);
14354         assertEquals(b1, b2);
14355         final Matrix ma1 = new Matrix(3, 3);
14356         ma1.setSubmatrix(0, 0,
14357                 2, 2,
14358                 new double[]{sx, myx, mzx,
14359                         mxy, sy, mzy,
14360                         mxz, myz, sz});
14361         assertEquals(calibrator.getInitialMa(), ma1);
14362         final Matrix ma2 = new Matrix(3, 3);
14363         calibrator.getInitialMa(ma2);
14364         assertEquals(ma1, ma2);
14365         assertNull(calibrator.getMeasurements());
14366         assertFalse(calibrator.isCommonAxisUsed());
14367         assertNull(calibrator.getListener());
14368         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14369         assertFalse(calibrator.isReady());
14370         assertFalse(calibrator.isRunning());
14371         assertNull(calibrator.getEstimatedBiases());
14372         assertFalse(calibrator.getEstimatedBiases(null));
14373         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14374         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14375         assertNull(calibrator.getEstimatedBiasFx());
14376         assertNull(calibrator.getEstimatedBiasFy());
14377         assertNull(calibrator.getEstimatedBiasFz());
14378         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14379         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14380         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14381         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14382         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14383         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14384         assertNull(calibrator.getEstimatedMa());
14385         assertNull(calibrator.getEstimatedSx());
14386         assertNull(calibrator.getEstimatedSy());
14387         assertNull(calibrator.getEstimatedSz());
14388         assertNull(calibrator.getEstimatedMxy());
14389         assertNull(calibrator.getEstimatedMxz());
14390         assertNull(calibrator.getEstimatedMyx());
14391         assertNull(calibrator.getEstimatedMyz());
14392         assertNull(calibrator.getEstimatedMzx());
14393         assertNull(calibrator.getEstimatedMzy());
14394         assertNull(calibrator.getEstimatedCovariance());
14395         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14396         assertNotNull(calibrator.getGroundTruthGravityNorm());
14397         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14398         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14399         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14400                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14401         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14402         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14403         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14404 
14405         // Force IllegalArgumentException
14406         calibrator = null;
14407         try {
14408             calibrator = new KnownGravityNormAccelerometerCalibrator(
14409                     -gravityNorm, biasX, biasY, biasZ, sx, sy, sz,
14410                     mxy, mxz, myx, myz, mzx, mzy);
14411             fail("IllegalArgumentException expected but not thrown");
14412         } catch (final IllegalArgumentException ignore) {
14413         }
14414         assertNull(calibrator);
14415     }
14416 
14417     @Test
14418     public void testConstructor119() throws WrongSizeException {
14419         final Collection<StandardDeviationBodyKinematics> measurements =
14420                 Collections.emptyList();
14421 
14422         final Matrix ba = generateBa();
14423         final double biasX = ba.getElementAtIndex(0);
14424         final double biasY = ba.getElementAtIndex(1);
14425         final double biasZ = ba.getElementAtIndex(2);
14426 
14427         final Matrix ma = generateMaCommonAxis();
14428         final double sx = ma.getElementAt(0, 0);
14429         final double sy = ma.getElementAt(1, 1);
14430         final double sz = ma.getElementAt(2, 2);
14431         final double mxy = ma.getElementAt(0, 1);
14432         final double mxz = ma.getElementAt(0, 2);
14433         final double myx = ma.getElementAt(1, 0);
14434         final double myz = ma.getElementAt(1, 2);
14435         final double mzx = ma.getElementAt(2, 0);
14436         final double mzy = ma.getElementAt(2, 1);
14437 
14438         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14439         final double latitude = Math.toRadians(
14440                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14441         final double longitude = Math.toRadians(
14442                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14443         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14444         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14445         final NEDVelocity nedVelocity = new NEDVelocity();
14446         final ECEFPosition ecefPosition = new ECEFPosition();
14447         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14448         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14449                 ecefPosition, ecefVelocity);
14450         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14451                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14452         final double gravityNorm = gravity.getNorm();
14453 
14454         KnownGravityNormAccelerometerCalibrator calibrator =
14455                 new KnownGravityNormAccelerometerCalibrator(
14456                         gravityNorm, measurements,
14457                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14458                         myx, myz, mzx, mzy);
14459 
14460         // check default values
14461         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14462         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14463         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14464         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14465         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14466         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14467         final Acceleration bx2 = new Acceleration(0.0,
14468                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14469         calibrator.getInitialBiasXAsAcceleration(bx2);
14470         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14471         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14472         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14473         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14474         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14475         final Acceleration by2 = new Acceleration(0.0,
14476                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14477         calibrator.getInitialBiasYAsAcceleration(by2);
14478         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14479         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14480         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14481         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14482         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14483         final Acceleration bz2 = new Acceleration(0.0,
14484                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14485         calibrator.getInitialBiasZAsAcceleration(bz2);
14486         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14487         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14488         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14489         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14490         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14491         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14492         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14493         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14494         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14495         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14496         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14497         final double[] bias1 = calibrator.getInitialBias();
14498         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14499         final double[] bias2 = new double[3];
14500         calibrator.getInitialBias(bias2);
14501         assertArrayEquals(bias1, bias2, 0.0);
14502         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14503         assertEquals(b1, ba);
14504         final Matrix b2 = new Matrix(3, 1);
14505         calibrator.getInitialBiasAsMatrix(b2);
14506         assertEquals(b1, b2);
14507         final Matrix ma1 = new Matrix(3, 3);
14508         ma1.setSubmatrix(0, 0,
14509                 2, 2,
14510                 new double[]{sx, myx, mzx,
14511                         mxy, sy, mzy,
14512                         mxz, myz, sz});
14513         assertEquals(calibrator.getInitialMa(), ma1);
14514         final Matrix ma2 = new Matrix(3, 3);
14515         calibrator.getInitialMa(ma2);
14516         assertEquals(ma1, ma2);
14517         assertSame(calibrator.getMeasurements(), measurements);
14518         assertFalse(calibrator.isCommonAxisUsed());
14519         assertNull(calibrator.getListener());
14520         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14521         assertFalse(calibrator.isReady());
14522         assertFalse(calibrator.isRunning());
14523         assertNull(calibrator.getEstimatedBiases());
14524         assertFalse(calibrator.getEstimatedBiases(null));
14525         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14526         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14527         assertNull(calibrator.getEstimatedBiasFx());
14528         assertNull(calibrator.getEstimatedBiasFy());
14529         assertNull(calibrator.getEstimatedBiasFz());
14530         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14531         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14532         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14533         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14534         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14535         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14536         assertNull(calibrator.getEstimatedMa());
14537         assertNull(calibrator.getEstimatedSx());
14538         assertNull(calibrator.getEstimatedSy());
14539         assertNull(calibrator.getEstimatedSz());
14540         assertNull(calibrator.getEstimatedMxy());
14541         assertNull(calibrator.getEstimatedMxz());
14542         assertNull(calibrator.getEstimatedMyx());
14543         assertNull(calibrator.getEstimatedMyz());
14544         assertNull(calibrator.getEstimatedMzx());
14545         assertNull(calibrator.getEstimatedMzy());
14546         assertNull(calibrator.getEstimatedCovariance());
14547         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14548         assertNotNull(calibrator.getGroundTruthGravityNorm());
14549         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14550         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14551         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14552                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14553         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14554         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14555         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14556 
14557         // Force IllegalArgumentException
14558         calibrator = null;
14559         try {
14560             calibrator = new KnownGravityNormAccelerometerCalibrator(
14561                     -gravityNorm, measurements,
14562                     biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14563                     myx, myz, mzx, mzy);
14564             fail("IllegalArgumentException expected but not thrown");
14565         } catch (final IllegalArgumentException ignore) {
14566         }
14567         assertNull(calibrator);
14568     }
14569 
14570     @Test
14571     public void testConstructor120() throws WrongSizeException {
14572         final Collection<StandardDeviationBodyKinematics> measurements =
14573                 Collections.emptyList();
14574 
14575         final Matrix ba = generateBa();
14576         final double biasX = ba.getElementAtIndex(0);
14577         final double biasY = ba.getElementAtIndex(1);
14578         final double biasZ = ba.getElementAtIndex(2);
14579 
14580         final Matrix ma = generateMaCommonAxis();
14581         final double sx = ma.getElementAt(0, 0);
14582         final double sy = ma.getElementAt(1, 1);
14583         final double sz = ma.getElementAt(2, 2);
14584         final double mxy = ma.getElementAt(0, 1);
14585         final double mxz = ma.getElementAt(0, 2);
14586         final double myx = ma.getElementAt(1, 0);
14587         final double myz = ma.getElementAt(1, 2);
14588         final double mzx = ma.getElementAt(2, 0);
14589         final double mzy = ma.getElementAt(2, 1);
14590 
14591         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14592         final double latitude = Math.toRadians(
14593                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14594         final double longitude = Math.toRadians(
14595                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14596         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14597         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14598         final NEDVelocity nedVelocity = new NEDVelocity();
14599         final ECEFPosition ecefPosition = new ECEFPosition();
14600         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14601         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14602                 ecefPosition, ecefVelocity);
14603         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14604                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14605         final double gravityNorm = gravity.getNorm();
14606 
14607         KnownGravityNormAccelerometerCalibrator calibrator =
14608                 new KnownGravityNormAccelerometerCalibrator(
14609                         gravityNorm, measurements,
14610                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14611                         myx, myz, mzx, mzy, this);
14612 
14613         // check default values
14614         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14615         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14616         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14617         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14618         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14619         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14620         final Acceleration bx2 = new Acceleration(0.0,
14621                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14622         calibrator.getInitialBiasXAsAcceleration(bx2);
14623         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14624         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14625         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14626         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14627         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14628         final Acceleration by2 = new Acceleration(0.0,
14629                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14630         calibrator.getInitialBiasYAsAcceleration(by2);
14631         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14632         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14633         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14634         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14635         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14636         final Acceleration bz2 = new Acceleration(0.0,
14637                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14638         calibrator.getInitialBiasZAsAcceleration(bz2);
14639         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14640         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14641         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14642         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14643         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14644         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14645         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14646         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14647         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14648         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14649         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14650         final double[] bias1 = calibrator.getInitialBias();
14651         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14652         final double[] bias2 = new double[3];
14653         calibrator.getInitialBias(bias2);
14654         assertArrayEquals(bias1, bias2, 0.0);
14655         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14656         assertEquals(b1, ba);
14657         final Matrix b2 = new Matrix(3, 1);
14658         calibrator.getInitialBiasAsMatrix(b2);
14659         assertEquals(b1, b2);
14660         final Matrix ma1 = new Matrix(3, 3);
14661         ma1.setSubmatrix(0, 0,
14662                 2, 2,
14663                 new double[]{sx, myx, mzx,
14664                         mxy, sy, mzy,
14665                         mxz, myz, sz});
14666         assertEquals(calibrator.getInitialMa(), ma1);
14667         final Matrix ma2 = new Matrix(3, 3);
14668         calibrator.getInitialMa(ma2);
14669         assertEquals(ma1, ma2);
14670         assertSame(calibrator.getMeasurements(), measurements);
14671         assertFalse(calibrator.isCommonAxisUsed());
14672         assertSame(calibrator.getListener(), this);
14673         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14674         assertFalse(calibrator.isReady());
14675         assertFalse(calibrator.isRunning());
14676         assertNull(calibrator.getEstimatedBiases());
14677         assertFalse(calibrator.getEstimatedBiases(null));
14678         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14679         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14680         assertNull(calibrator.getEstimatedBiasFx());
14681         assertNull(calibrator.getEstimatedBiasFy());
14682         assertNull(calibrator.getEstimatedBiasFz());
14683         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14684         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14685         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14686         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14687         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14688         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14689         assertNull(calibrator.getEstimatedMa());
14690         assertNull(calibrator.getEstimatedSx());
14691         assertNull(calibrator.getEstimatedSy());
14692         assertNull(calibrator.getEstimatedSz());
14693         assertNull(calibrator.getEstimatedMxy());
14694         assertNull(calibrator.getEstimatedMxz());
14695         assertNull(calibrator.getEstimatedMyx());
14696         assertNull(calibrator.getEstimatedMyz());
14697         assertNull(calibrator.getEstimatedMzx());
14698         assertNull(calibrator.getEstimatedMzy());
14699         assertNull(calibrator.getEstimatedCovariance());
14700         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14701         assertNotNull(calibrator.getGroundTruthGravityNorm());
14702         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14703         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14704         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14705                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14706         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14707         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14708         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14709 
14710         // Force IllegalArgumentException
14711         calibrator = null;
14712         try {
14713             calibrator = new KnownGravityNormAccelerometerCalibrator(
14714                     -gravityNorm, measurements,
14715                     biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14716                     myx, myz, mzx, mzy, this);
14717             fail("IllegalArgumentException expected but not thrown");
14718         } catch (final IllegalArgumentException ignore) {
14719         }
14720         assertNull(calibrator);
14721     }
14722 
14723     @Test
14724     public void testConstructor121() throws WrongSizeException {
14725         final Matrix ba = generateBa();
14726         final double biasX = ba.getElementAtIndex(0);
14727         final double biasY = ba.getElementAtIndex(1);
14728         final double biasZ = ba.getElementAtIndex(2);
14729 
14730         final Matrix ma = generateMaCommonAxis();
14731         final double sx = ma.getElementAt(0, 0);
14732         final double sy = ma.getElementAt(1, 1);
14733         final double sz = ma.getElementAt(2, 2);
14734         final double mxy = ma.getElementAt(0, 1);
14735         final double mxz = ma.getElementAt(0, 2);
14736         final double myx = ma.getElementAt(1, 0);
14737         final double myz = ma.getElementAt(1, 2);
14738         final double mzx = ma.getElementAt(2, 0);
14739         final double mzy = ma.getElementAt(2, 1);
14740 
14741         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14742         final double latitude = Math.toRadians(
14743                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14744         final double longitude = Math.toRadians(
14745                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14746         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14747         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14748         final NEDVelocity nedVelocity = new NEDVelocity();
14749         final ECEFPosition ecefPosition = new ECEFPosition();
14750         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14751         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14752                 ecefPosition, ecefVelocity);
14753         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14754                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14755         final double gravityNorm = gravity.getNorm();
14756 
14757         KnownGravityNormAccelerometerCalibrator calibrator =
14758                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
14759                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14760                         myx, myz, mzx, mzy);
14761 
14762         // check default values
14763         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14764         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14765         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14766         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14767         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14768         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14769         final Acceleration bx2 = new Acceleration(0.0,
14770                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14771         calibrator.getInitialBiasXAsAcceleration(bx2);
14772         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14773         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14774         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14775         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14776         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14777         final Acceleration by2 = new Acceleration(0.0,
14778                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14779         calibrator.getInitialBiasYAsAcceleration(by2);
14780         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14781         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14782         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14783         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14784         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14785         final Acceleration bz2 = new Acceleration(0.0,
14786                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14787         calibrator.getInitialBiasZAsAcceleration(bz2);
14788         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14789         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14790         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14791         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14792         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14793         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14794         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14795         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14796         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14797         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14798         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14799         final double[] bias1 = calibrator.getInitialBias();
14800         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14801         final double[] bias2 = new double[3];
14802         calibrator.getInitialBias(bias2);
14803         assertArrayEquals(bias1, bias2, 0.0);
14804         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14805         assertEquals(b1, ba);
14806         final Matrix b2 = new Matrix(3, 1);
14807         calibrator.getInitialBiasAsMatrix(b2);
14808         assertEquals(b1, b2);
14809         final Matrix ma1 = new Matrix(3, 3);
14810         ma1.setSubmatrix(0, 0,
14811                 2, 2,
14812                 new double[]{sx, myx, mzx,
14813                         mxy, sy, mzy,
14814                         mxz, myz, sz});
14815         assertEquals(calibrator.getInitialMa(), ma1);
14816         final Matrix ma2 = new Matrix(3, 3);
14817         calibrator.getInitialMa(ma2);
14818         assertEquals(ma1, ma2);
14819         assertNull(calibrator.getMeasurements());
14820         assertTrue(calibrator.isCommonAxisUsed());
14821         assertNull(calibrator.getListener());
14822         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14823         assertFalse(calibrator.isReady());
14824         assertFalse(calibrator.isRunning());
14825         assertNull(calibrator.getEstimatedBiases());
14826         assertFalse(calibrator.getEstimatedBiases(null));
14827         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14828         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14829         assertNull(calibrator.getEstimatedBiasFx());
14830         assertNull(calibrator.getEstimatedBiasFy());
14831         assertNull(calibrator.getEstimatedBiasFz());
14832         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14833         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14834         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14835         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14836         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14837         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14838         assertNull(calibrator.getEstimatedMa());
14839         assertNull(calibrator.getEstimatedSx());
14840         assertNull(calibrator.getEstimatedSy());
14841         assertNull(calibrator.getEstimatedSz());
14842         assertNull(calibrator.getEstimatedMxy());
14843         assertNull(calibrator.getEstimatedMxz());
14844         assertNull(calibrator.getEstimatedMyx());
14845         assertNull(calibrator.getEstimatedMyz());
14846         assertNull(calibrator.getEstimatedMzx());
14847         assertNull(calibrator.getEstimatedMzy());
14848         assertNull(calibrator.getEstimatedCovariance());
14849         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14850         assertNotNull(calibrator.getGroundTruthGravityNorm());
14851         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14852         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14853         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14854                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14855         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14856         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14857         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14858 
14859         // Force IllegalArgumentException
14860         calibrator = null;
14861         try {
14862             calibrator = new KnownGravityNormAccelerometerCalibrator(
14863                     -gravityNorm, true, biasX, biasY, biasZ,
14864                     sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
14865             fail("IllegalArgumentException expected but not thrown");
14866         } catch (final IllegalArgumentException ignore) {
14867         }
14868         assertNull(calibrator);
14869     }
14870 
14871     @Test
14872     public void testConstructor122() throws WrongSizeException {
14873         final Matrix ba = generateBa();
14874         final double biasX = ba.getElementAtIndex(0);
14875         final double biasY = ba.getElementAtIndex(1);
14876         final double biasZ = ba.getElementAtIndex(2);
14877 
14878         final Matrix ma = generateMaCommonAxis();
14879         final double sx = ma.getElementAt(0, 0);
14880         final double sy = ma.getElementAt(1, 1);
14881         final double sz = ma.getElementAt(2, 2);
14882         final double mxy = ma.getElementAt(0, 1);
14883         final double mxz = ma.getElementAt(0, 2);
14884         final double myx = ma.getElementAt(1, 0);
14885         final double myz = ma.getElementAt(1, 2);
14886         final double mzx = ma.getElementAt(2, 0);
14887         final double mzy = ma.getElementAt(2, 1);
14888 
14889         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14890         final double latitude = Math.toRadians(
14891                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14892         final double longitude = Math.toRadians(
14893                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14894         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14895         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14896         final NEDVelocity nedVelocity = new NEDVelocity();
14897         final ECEFPosition ecefPosition = new ECEFPosition();
14898         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14899         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14900                 ecefPosition, ecefVelocity);
14901         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14902                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14903         final double gravityNorm = gravity.getNorm();
14904 
14905         KnownGravityNormAccelerometerCalibrator calibrator =
14906                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
14907                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14908                         myx, myz, mzx, mzy, this);
14909 
14910         // check default values
14911         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14912         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14913         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14914         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14915         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14916         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14917         final Acceleration bx2 = new Acceleration(0.0,
14918                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14919         calibrator.getInitialBiasXAsAcceleration(bx2);
14920         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14921         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14922         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14923         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14924         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14925         final Acceleration by2 = new Acceleration(0.0,
14926                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14927         calibrator.getInitialBiasYAsAcceleration(by2);
14928         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14929         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14930         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14931         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14932         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14933         final Acceleration bz2 = new Acceleration(0.0,
14934                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14935         calibrator.getInitialBiasZAsAcceleration(bz2);
14936         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14937         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14938         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14939         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14940         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14941         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14942         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14943         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14944         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14945         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14946         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14947         final double[] bias1 = calibrator.getInitialBias();
14948         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14949         final double[] bias2 = new double[3];
14950         calibrator.getInitialBias(bias2);
14951         assertArrayEquals(bias1, bias2, 0.0);
14952         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14953         assertEquals(b1, ba);
14954         final Matrix b2 = new Matrix(3, 1);
14955         calibrator.getInitialBiasAsMatrix(b2);
14956         assertEquals(b1, b2);
14957         final Matrix ma1 = new Matrix(3, 3);
14958         ma1.setSubmatrix(0, 0,
14959                 2, 2,
14960                 new double[]{sx, myx, mzx,
14961                         mxy, sy, mzy,
14962                         mxz, myz, sz});
14963         assertEquals(calibrator.getInitialMa(), ma1);
14964         final Matrix ma2 = new Matrix(3, 3);
14965         calibrator.getInitialMa(ma2);
14966         assertEquals(ma1, ma2);
14967         assertNull(calibrator.getMeasurements());
14968         assertTrue(calibrator.isCommonAxisUsed());
14969         assertSame(calibrator.getListener(), this);
14970         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14971         assertFalse(calibrator.isReady());
14972         assertFalse(calibrator.isRunning());
14973         assertNull(calibrator.getEstimatedBiases());
14974         assertFalse(calibrator.getEstimatedBiases(null));
14975         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14976         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14977         assertNull(calibrator.getEstimatedBiasFx());
14978         assertNull(calibrator.getEstimatedBiasFy());
14979         assertNull(calibrator.getEstimatedBiasFz());
14980         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14981         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14982         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14983         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14984         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14985         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14986         assertNull(calibrator.getEstimatedMa());
14987         assertNull(calibrator.getEstimatedSx());
14988         assertNull(calibrator.getEstimatedSy());
14989         assertNull(calibrator.getEstimatedSz());
14990         assertNull(calibrator.getEstimatedMxy());
14991         assertNull(calibrator.getEstimatedMxz());
14992         assertNull(calibrator.getEstimatedMyx());
14993         assertNull(calibrator.getEstimatedMyz());
14994         assertNull(calibrator.getEstimatedMzx());
14995         assertNull(calibrator.getEstimatedMzy());
14996         assertNull(calibrator.getEstimatedCovariance());
14997         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14998         assertNotNull(calibrator.getGroundTruthGravityNorm());
14999         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15000         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15001         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15002                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15003         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15004         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15005         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15006 
15007         // Force IllegalArgumentException
15008         calibrator = null;
15009         try {
15010             calibrator = new KnownGravityNormAccelerometerCalibrator(
15011                     -gravityNorm, true, biasX, biasY, biasZ, sx, sy, sz,
15012                     mxy, mxz, myx, myz, mzx, mzy, this);
15013             fail("IllegalArgumentException expected but not thrown");
15014         } catch (final IllegalArgumentException ignore) {
15015         }
15016         assertNull(calibrator);
15017     }
15018 
15019     @Test
15020     public void testConstructor123() throws WrongSizeException {
15021         final Collection<StandardDeviationBodyKinematics> measurements =
15022                 Collections.emptyList();
15023 
15024         final Matrix ba = generateBa();
15025         final double biasX = ba.getElementAtIndex(0);
15026         final double biasY = ba.getElementAtIndex(1);
15027         final double biasZ = ba.getElementAtIndex(2);
15028 
15029         final Matrix ma = generateMaCommonAxis();
15030         final double sx = ma.getElementAt(0, 0);
15031         final double sy = ma.getElementAt(1, 1);
15032         final double sz = ma.getElementAt(2, 2);
15033         final double mxy = ma.getElementAt(0, 1);
15034         final double mxz = ma.getElementAt(0, 2);
15035         final double myx = ma.getElementAt(1, 0);
15036         final double myz = ma.getElementAt(1, 2);
15037         final double mzx = ma.getElementAt(2, 0);
15038         final double mzy = ma.getElementAt(2, 1);
15039 
15040         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15041         final double latitude = Math.toRadians(
15042                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15043         final double longitude = Math.toRadians(
15044                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15045         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15046         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15047         final NEDVelocity nedVelocity = new NEDVelocity();
15048         final ECEFPosition ecefPosition = new ECEFPosition();
15049         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15050         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15051                 ecefPosition, ecefVelocity);
15052         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15053                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15054         final double gravityNorm = gravity.getNorm();
15055 
15056         KnownGravityNormAccelerometerCalibrator calibrator =
15057                 new KnownGravityNormAccelerometerCalibrator(
15058                         gravityNorm, measurements,
15059                         true, biasX, biasY, biasZ,
15060                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
15061 
15062         // check default values
15063         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15064         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15065         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15066         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15067         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15068         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15069         final Acceleration bx2 = new Acceleration(0.0,
15070                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15071         calibrator.getInitialBiasXAsAcceleration(bx2);
15072         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15073         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15074         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15075         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15076         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15077         final Acceleration by2 = new Acceleration(0.0,
15078                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15079         calibrator.getInitialBiasYAsAcceleration(by2);
15080         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15081         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15082         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15083         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15084         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15085         final Acceleration bz2 = new Acceleration(0.0,
15086                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15087         calibrator.getInitialBiasZAsAcceleration(bz2);
15088         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15089         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15090         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15091         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15092         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15093         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15094         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15095         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15096         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15097         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15098         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15099         final double[] bias1 = calibrator.getInitialBias();
15100         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15101         final double[] bias2 = new double[3];
15102         calibrator.getInitialBias(bias2);
15103         assertArrayEquals(bias1, bias2, 0.0);
15104         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15105         assertEquals(b1, ba);
15106         final Matrix b2 = new Matrix(3, 1);
15107         calibrator.getInitialBiasAsMatrix(b2);
15108         assertEquals(b1, b2);
15109         final Matrix ma1 = new Matrix(3, 3);
15110         ma1.setSubmatrix(0, 0,
15111                 2, 2,
15112                 new double[]{sx, myx, mzx,
15113                         mxy, sy, mzy,
15114                         mxz, myz, sz});
15115         assertEquals(calibrator.getInitialMa(), ma1);
15116         final Matrix ma2 = new Matrix(3, 3);
15117         calibrator.getInitialMa(ma2);
15118         assertEquals(ma1, ma2);
15119         assertSame(calibrator.getMeasurements(), measurements);
15120         assertTrue(calibrator.isCommonAxisUsed());
15121         assertNull(calibrator.getListener());
15122         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15123         assertFalse(calibrator.isReady());
15124         assertFalse(calibrator.isRunning());
15125         assertNull(calibrator.getEstimatedBiases());
15126         assertFalse(calibrator.getEstimatedBiases(null));
15127         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15128         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15129         assertNull(calibrator.getEstimatedBiasFx());
15130         assertNull(calibrator.getEstimatedBiasFy());
15131         assertNull(calibrator.getEstimatedBiasFz());
15132         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15133         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15134         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15135         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15136         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15137         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15138         assertNull(calibrator.getEstimatedMa());
15139         assertNull(calibrator.getEstimatedSx());
15140         assertNull(calibrator.getEstimatedSy());
15141         assertNull(calibrator.getEstimatedSz());
15142         assertNull(calibrator.getEstimatedMxy());
15143         assertNull(calibrator.getEstimatedMxz());
15144         assertNull(calibrator.getEstimatedMyx());
15145         assertNull(calibrator.getEstimatedMyz());
15146         assertNull(calibrator.getEstimatedMzx());
15147         assertNull(calibrator.getEstimatedMzy());
15148         assertNull(calibrator.getEstimatedCovariance());
15149         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15150         assertNotNull(calibrator.getGroundTruthGravityNorm());
15151         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15152         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15153         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15154                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15155         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15156         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15157         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15158 
15159         // Force IllegalArgumentException
15160         calibrator = null;
15161         try {
15162             calibrator = new KnownGravityNormAccelerometerCalibrator(
15163                     -gravityNorm, measurements,
15164                     true, biasX, biasY, biasZ,
15165                     sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
15166             fail("IllegalArgumentException expected but not thrown");
15167         } catch (final IllegalArgumentException ignore) {
15168         }
15169         assertNull(calibrator);
15170     }
15171 
15172     @Test
15173     public void testConstructor124() throws WrongSizeException {
15174         final Collection<StandardDeviationBodyKinematics> measurements =
15175                 Collections.emptyList();
15176 
15177         final Matrix ba = generateBa();
15178         final double biasX = ba.getElementAtIndex(0);
15179         final double biasY = ba.getElementAtIndex(1);
15180         final double biasZ = ba.getElementAtIndex(2);
15181 
15182         final Matrix ma = generateMaCommonAxis();
15183         final double sx = ma.getElementAt(0, 0);
15184         final double sy = ma.getElementAt(1, 1);
15185         final double sz = ma.getElementAt(2, 2);
15186         final double mxy = ma.getElementAt(0, 1);
15187         final double mxz = ma.getElementAt(0, 2);
15188         final double myx = ma.getElementAt(1, 0);
15189         final double myz = ma.getElementAt(1, 2);
15190         final double mzx = ma.getElementAt(2, 0);
15191         final double mzy = ma.getElementAt(2, 1);
15192 
15193         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15194         final double latitude = Math.toRadians(
15195                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15196         final double longitude = Math.toRadians(
15197                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15198         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15199         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15200         final NEDVelocity nedVelocity = new NEDVelocity();
15201         final ECEFPosition ecefPosition = new ECEFPosition();
15202         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15203         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15204                 ecefPosition, ecefVelocity);
15205         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15206                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15207         final double gravityNorm = gravity.getNorm();
15208 
15209         KnownGravityNormAccelerometerCalibrator calibrator =
15210                 new KnownGravityNormAccelerometerCalibrator(
15211                         gravityNorm, measurements,
15212                         true, biasX, biasY, biasZ,
15213                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
15214                         this);
15215 
15216         // check default values
15217         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15218         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15219         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15220         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15221         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15222         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15223         final Acceleration bx2 = new Acceleration(0.0,
15224                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15225         calibrator.getInitialBiasXAsAcceleration(bx2);
15226         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15227         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15228         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15229         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15230         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15231         final Acceleration by2 = new Acceleration(0.0,
15232                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15233         calibrator.getInitialBiasYAsAcceleration(by2);
15234         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15235         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15236         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15237         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15238         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15239         final Acceleration bz2 = new Acceleration(0.0,
15240                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15241         calibrator.getInitialBiasZAsAcceleration(bz2);
15242         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15243         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15244         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15245         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15246         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15247         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15248         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15249         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15250         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15251         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15252         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15253         final double[] bias1 = calibrator.getInitialBias();
15254         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15255         final double[] bias2 = new double[3];
15256         calibrator.getInitialBias(bias2);
15257         assertArrayEquals(bias1, bias2, 0.0);
15258         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15259         assertEquals(b1, ba);
15260         final Matrix b2 = new Matrix(3, 1);
15261         calibrator.getInitialBiasAsMatrix(b2);
15262         assertEquals(b1, b2);
15263         final Matrix ma1 = new Matrix(3, 3);
15264         ma1.setSubmatrix(0, 0,
15265                 2, 2,
15266                 new double[]{sx, myx, mzx,
15267                         mxy, sy, mzy,
15268                         mxz, myz, sz});
15269         assertEquals(calibrator.getInitialMa(), ma1);
15270         final Matrix ma2 = new Matrix(3, 3);
15271         calibrator.getInitialMa(ma2);
15272         assertEquals(ma1, ma2);
15273         assertSame(calibrator.getMeasurements(), measurements);
15274         assertTrue(calibrator.isCommonAxisUsed());
15275         assertSame(calibrator.getListener(), this);
15276         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15277         assertFalse(calibrator.isReady());
15278         assertFalse(calibrator.isRunning());
15279         assertNull(calibrator.getEstimatedBiases());
15280         assertFalse(calibrator.getEstimatedBiases(null));
15281         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15282         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15283         assertNull(calibrator.getEstimatedBiasFx());
15284         assertNull(calibrator.getEstimatedBiasFy());
15285         assertNull(calibrator.getEstimatedBiasFz());
15286         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15287         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15288         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15289         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15290         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15291         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15292         assertNull(calibrator.getEstimatedMa());
15293         assertNull(calibrator.getEstimatedSx());
15294         assertNull(calibrator.getEstimatedSy());
15295         assertNull(calibrator.getEstimatedSz());
15296         assertNull(calibrator.getEstimatedMxy());
15297         assertNull(calibrator.getEstimatedMxz());
15298         assertNull(calibrator.getEstimatedMyx());
15299         assertNull(calibrator.getEstimatedMyz());
15300         assertNull(calibrator.getEstimatedMzx());
15301         assertNull(calibrator.getEstimatedMzy());
15302         assertNull(calibrator.getEstimatedCovariance());
15303         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15304         assertNotNull(calibrator.getGroundTruthGravityNorm());
15305         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15306         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15307         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15308                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15309         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15310         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15311         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15312 
15313         // Force IllegalArgumentException
15314         calibrator = null;
15315         try {
15316             calibrator = new KnownGravityNormAccelerometerCalibrator(
15317                     -gravityNorm, measurements,
15318                     true, biasX, biasY, biasZ,
15319                     sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
15320                     this);
15321             fail("IllegalArgumentException expected but not thrown");
15322         } catch (final IllegalArgumentException ignore) {
15323         }
15324         assertNull(calibrator);
15325     }
15326 
15327     @Test
15328     public void testConstructor125() throws WrongSizeException {
15329         final Matrix ba = generateBa();
15330         final double biasX = ba.getElementAtIndex(0);
15331         final double biasY = ba.getElementAtIndex(1);
15332         final double biasZ = ba.getElementAtIndex(2);
15333 
15334         final Acceleration bx = new Acceleration(biasX,
15335                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15336         final Acceleration by = new Acceleration(biasY,
15337                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15338         final Acceleration bz = new Acceleration(biasZ,
15339                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15340 
15341         final Matrix ma = generateMaCommonAxis();
15342         final double sx = ma.getElementAt(0, 0);
15343         final double sy = ma.getElementAt(1, 1);
15344         final double sz = ma.getElementAt(2, 2);
15345         final double mxy = ma.getElementAt(0, 1);
15346         final double mxz = ma.getElementAt(0, 2);
15347         final double myx = ma.getElementAt(1, 0);
15348         final double myz = ma.getElementAt(1, 2);
15349         final double mzx = ma.getElementAt(2, 0);
15350         final double mzy = ma.getElementAt(2, 1);
15351 
15352         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15353         final double latitude = Math.toRadians(
15354                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15355         final double longitude = Math.toRadians(
15356                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15357         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15358         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15359         final NEDVelocity nedVelocity = new NEDVelocity();
15360         final ECEFPosition ecefPosition = new ECEFPosition();
15361         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15362         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15363                 ecefPosition, ecefVelocity);
15364         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15365                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15366         final double gravityNorm = gravity.getNorm();
15367 
15368         KnownGravityNormAccelerometerCalibrator calibrator =
15369                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
15370                         bx, by, bz, sx, sy, sz, mxy, mxz,
15371                         myx, myz, mzx, mzy);
15372 
15373         // check default values
15374         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15375         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15376         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15377         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15378         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15379         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15380         final Acceleration bx2 = new Acceleration(0.0,
15381                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15382         calibrator.getInitialBiasXAsAcceleration(bx2);
15383         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15384         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15385         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15386         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15387         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15388         final Acceleration by2 = new Acceleration(0.0,
15389                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15390         calibrator.getInitialBiasYAsAcceleration(by2);
15391         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15392         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15393         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15394         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15395         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15396         final Acceleration bz2 = new Acceleration(0.0,
15397                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15398         calibrator.getInitialBiasZAsAcceleration(bz2);
15399         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15400         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15401         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15402         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15403         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15404         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15405         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15406         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15407         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15408         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15409         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15410         final double[] bias1 = calibrator.getInitialBias();
15411         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15412         final double[] bias2 = new double[3];
15413         calibrator.getInitialBias(bias2);
15414         assertArrayEquals(bias1, bias2, 0.0);
15415         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15416         assertEquals(b1, ba);
15417         final Matrix b2 = new Matrix(3, 1);
15418         calibrator.getInitialBiasAsMatrix(b2);
15419         assertEquals(b1, b2);
15420         final Matrix ma1 = new Matrix(3, 3);
15421         ma1.setSubmatrix(0, 0,
15422                 2, 2,
15423                 new double[]{sx, myx, mzx,
15424                         mxy, sy, mzy,
15425                         mxz, myz, sz});
15426         assertEquals(calibrator.getInitialMa(), ma1);
15427         final Matrix ma2 = new Matrix(3, 3);
15428         calibrator.getInitialMa(ma2);
15429         assertEquals(ma1, ma2);
15430         assertNull(calibrator.getMeasurements());
15431         assertFalse(calibrator.isCommonAxisUsed());
15432         assertNull(calibrator.getListener());
15433         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15434         assertFalse(calibrator.isReady());
15435         assertFalse(calibrator.isRunning());
15436         assertNull(calibrator.getEstimatedBiases());
15437         assertFalse(calibrator.getEstimatedBiases(null));
15438         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15439         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15440         assertNull(calibrator.getEstimatedBiasFx());
15441         assertNull(calibrator.getEstimatedBiasFy());
15442         assertNull(calibrator.getEstimatedBiasFz());
15443         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15444         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15445         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15446         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15447         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15448         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15449         assertNull(calibrator.getEstimatedMa());
15450         assertNull(calibrator.getEstimatedSx());
15451         assertNull(calibrator.getEstimatedSy());
15452         assertNull(calibrator.getEstimatedSz());
15453         assertNull(calibrator.getEstimatedMxy());
15454         assertNull(calibrator.getEstimatedMxz());
15455         assertNull(calibrator.getEstimatedMyx());
15456         assertNull(calibrator.getEstimatedMyz());
15457         assertNull(calibrator.getEstimatedMzx());
15458         assertNull(calibrator.getEstimatedMzy());
15459         assertNull(calibrator.getEstimatedCovariance());
15460         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15461         assertNotNull(calibrator.getGroundTruthGravityNorm());
15462         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15463         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15464         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15465                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15466         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15467         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15468         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15469 
15470         // Force IllegalArgumentException
15471         calibrator = null;
15472         try {
15473             calibrator = new KnownGravityNormAccelerometerCalibrator(
15474                     -gravityNorm, bx, by, bz, sx, sy, sz, mxy, mxz,
15475                     myx, myz, mzx, mzy);
15476             fail("IllegalArgumentException expected but not thrown");
15477         } catch (final IllegalArgumentException ignore) {
15478         }
15479         assertNull(calibrator);
15480     }
15481 
15482     @Test
15483     public void testConstructor126() throws WrongSizeException {
15484         final Matrix ba = generateBa();
15485         final double biasX = ba.getElementAtIndex(0);
15486         final double biasY = ba.getElementAtIndex(1);
15487         final double biasZ = ba.getElementAtIndex(2);
15488 
15489         final Acceleration bx = new Acceleration(biasX,
15490                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15491         final Acceleration by = new Acceleration(biasY,
15492                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15493         final Acceleration bz = new Acceleration(biasZ,
15494                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15495 
15496         final Matrix ma = generateMaCommonAxis();
15497         final double sx = ma.getElementAt(0, 0);
15498         final double sy = ma.getElementAt(1, 1);
15499         final double sz = ma.getElementAt(2, 2);
15500         final double mxy = ma.getElementAt(0, 1);
15501         final double mxz = ma.getElementAt(0, 2);
15502         final double myx = ma.getElementAt(1, 0);
15503         final double myz = ma.getElementAt(1, 2);
15504         final double mzx = ma.getElementAt(2, 0);
15505         final double mzy = ma.getElementAt(2, 1);
15506 
15507         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15508         final double latitude = Math.toRadians(
15509                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15510         final double longitude = Math.toRadians(
15511                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15512         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15513         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15514         final NEDVelocity nedVelocity = new NEDVelocity();
15515         final ECEFPosition ecefPosition = new ECEFPosition();
15516         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15517         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15518                 ecefPosition, ecefVelocity);
15519         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15520                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15521         final double gravityNorm = gravity.getNorm();
15522 
15523         KnownGravityNormAccelerometerCalibrator calibrator =
15524                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
15525                         bx, by, bz, sx, sy, sz, mxy, mxz,
15526                         myx, myz, mzx, mzy, this);
15527 
15528         // check default values
15529         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15530         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15531         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15532         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15533         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15534         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15535         final Acceleration bx2 = new Acceleration(0.0,
15536                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15537         calibrator.getInitialBiasXAsAcceleration(bx2);
15538         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15539         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15540         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15541         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15542         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15543         final Acceleration by2 = new Acceleration(0.0,
15544                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15545         calibrator.getInitialBiasYAsAcceleration(by2);
15546         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15547         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15548         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15549         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15550         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15551         final Acceleration bz2 = new Acceleration(0.0,
15552                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15553         calibrator.getInitialBiasZAsAcceleration(bz2);
15554         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15555         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15556         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15557         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15558         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15559         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15560         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15561         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15562         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15563         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15564         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15565         final double[] bias1 = calibrator.getInitialBias();
15566         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15567         final double[] bias2 = new double[3];
15568         calibrator.getInitialBias(bias2);
15569         assertArrayEquals(bias1, bias2, 0.0);
15570         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15571         assertEquals(b1, ba);
15572         final Matrix b2 = new Matrix(3, 1);
15573         calibrator.getInitialBiasAsMatrix(b2);
15574         assertEquals(b1, b2);
15575         final Matrix ma1 = new Matrix(3, 3);
15576         ma1.setSubmatrix(0, 0,
15577                 2, 2,
15578                 new double[]{sx, myx, mzx,
15579                         mxy, sy, mzy,
15580                         mxz, myz, sz});
15581         assertEquals(calibrator.getInitialMa(), ma1);
15582         final Matrix ma2 = new Matrix(3, 3);
15583         calibrator.getInitialMa(ma2);
15584         assertEquals(ma1, ma2);
15585         assertNull(calibrator.getMeasurements());
15586         assertFalse(calibrator.isCommonAxisUsed());
15587         assertSame(calibrator.getListener(), this);
15588         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15589         assertFalse(calibrator.isReady());
15590         assertFalse(calibrator.isRunning());
15591         assertNull(calibrator.getEstimatedBiases());
15592         assertFalse(calibrator.getEstimatedBiases(null));
15593         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15594         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15595         assertNull(calibrator.getEstimatedBiasFx());
15596         assertNull(calibrator.getEstimatedBiasFy());
15597         assertNull(calibrator.getEstimatedBiasFz());
15598         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15599         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15600         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15601         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15602         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15603         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15604         assertNull(calibrator.getEstimatedMa());
15605         assertNull(calibrator.getEstimatedSx());
15606         assertNull(calibrator.getEstimatedSy());
15607         assertNull(calibrator.getEstimatedSz());
15608         assertNull(calibrator.getEstimatedMxy());
15609         assertNull(calibrator.getEstimatedMxz());
15610         assertNull(calibrator.getEstimatedMyx());
15611         assertNull(calibrator.getEstimatedMyz());
15612         assertNull(calibrator.getEstimatedMzx());
15613         assertNull(calibrator.getEstimatedMzy());
15614         assertNull(calibrator.getEstimatedCovariance());
15615         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15616         assertNotNull(calibrator.getGroundTruthGravityNorm());
15617         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15618         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15619         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15620                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15621         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15622         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15623         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15624 
15625         // Force IllegalArgumentException
15626         calibrator = null;
15627         try {
15628             calibrator = new KnownGravityNormAccelerometerCalibrator(
15629                     -gravityNorm, bx, by, bz, sx, sy, sz, mxy, mxz,
15630                     myx, myz, mzx, mzy, this);
15631             fail("IllegalArgumentException expected but not thrown");
15632         } catch (final IllegalArgumentException ignore) {
15633         }
15634         assertNull(calibrator);
15635     }
15636 
15637     @Test
15638     public void testConstructor127() throws WrongSizeException {
15639         final Collection<StandardDeviationBodyKinematics> measurements =
15640                 Collections.emptyList();
15641 
15642         final Matrix ba = generateBa();
15643         final double biasX = ba.getElementAtIndex(0);
15644         final double biasY = ba.getElementAtIndex(1);
15645         final double biasZ = ba.getElementAtIndex(2);
15646 
15647         final Acceleration bx = new Acceleration(biasX,
15648                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15649         final Acceleration by = new Acceleration(biasY,
15650                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15651         final Acceleration bz = new Acceleration(biasZ,
15652                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15653 
15654         final Matrix ma = generateMaCommonAxis();
15655         final double sx = ma.getElementAt(0, 0);
15656         final double sy = ma.getElementAt(1, 1);
15657         final double sz = ma.getElementAt(2, 2);
15658         final double mxy = ma.getElementAt(0, 1);
15659         final double mxz = ma.getElementAt(0, 2);
15660         final double myx = ma.getElementAt(1, 0);
15661         final double myz = ma.getElementAt(1, 2);
15662         final double mzx = ma.getElementAt(2, 0);
15663         final double mzy = ma.getElementAt(2, 1);
15664 
15665         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15666         final double latitude = Math.toRadians(
15667                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15668         final double longitude = Math.toRadians(
15669                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15670         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15671         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15672         final NEDVelocity nedVelocity = new NEDVelocity();
15673         final ECEFPosition ecefPosition = new ECEFPosition();
15674         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15675         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15676                 ecefPosition, ecefVelocity);
15677         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15678                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15679         final double gravityNorm = gravity.getNorm();
15680 
15681         KnownGravityNormAccelerometerCalibrator calibrator =
15682                 new KnownGravityNormAccelerometerCalibrator(
15683                         gravityNorm, measurements,
15684                         bx, by, bz, sx, sy, sz, mxy, mxz,
15685                         myx, myz, mzx, mzy);
15686 
15687         // check default values
15688         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15689         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15690         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15691         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15692         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15693         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15694         final Acceleration bx2 = new Acceleration(0.0,
15695                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15696         calibrator.getInitialBiasXAsAcceleration(bx2);
15697         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15698         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15699         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15700         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15701         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15702         final Acceleration by2 = new Acceleration(0.0,
15703                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15704         calibrator.getInitialBiasYAsAcceleration(by2);
15705         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15706         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15707         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15708         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15709         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15710         final Acceleration bz2 = new Acceleration(0.0,
15711                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15712         calibrator.getInitialBiasZAsAcceleration(bz2);
15713         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15714         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15715         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15716         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15717         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15718         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15719         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15720         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15721         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15722         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15723         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15724         final double[] bias1 = calibrator.getInitialBias();
15725         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15726         final double[] bias2 = new double[3];
15727         calibrator.getInitialBias(bias2);
15728         assertArrayEquals(bias1, bias2, 0.0);
15729         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15730         assertEquals(b1, ba);
15731         final Matrix b2 = new Matrix(3, 1);
15732         calibrator.getInitialBiasAsMatrix(b2);
15733         assertEquals(b1, b2);
15734         final Matrix ma1 = new Matrix(3, 3);
15735         ma1.setSubmatrix(0, 0,
15736                 2, 2,
15737                 new double[]{sx, myx, mzx,
15738                         mxy, sy, mzy,
15739                         mxz, myz, sz});
15740         assertEquals(calibrator.getInitialMa(), ma1);
15741         final Matrix ma2 = new Matrix(3, 3);
15742         calibrator.getInitialMa(ma2);
15743         assertEquals(ma1, ma2);
15744         assertSame(calibrator.getMeasurements(), measurements);
15745         assertFalse(calibrator.isCommonAxisUsed());
15746         assertNull(calibrator.getListener());
15747         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15748         assertFalse(calibrator.isReady());
15749         assertFalse(calibrator.isRunning());
15750         assertNull(calibrator.getEstimatedBiases());
15751         assertFalse(calibrator.getEstimatedBiases(null));
15752         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15753         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15754         assertNull(calibrator.getEstimatedBiasFx());
15755         assertNull(calibrator.getEstimatedBiasFy());
15756         assertNull(calibrator.getEstimatedBiasFz());
15757         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15758         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15759         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15760         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15761         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15762         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15763         assertNull(calibrator.getEstimatedMa());
15764         assertNull(calibrator.getEstimatedSx());
15765         assertNull(calibrator.getEstimatedSy());
15766         assertNull(calibrator.getEstimatedSz());
15767         assertNull(calibrator.getEstimatedMxy());
15768         assertNull(calibrator.getEstimatedMxz());
15769         assertNull(calibrator.getEstimatedMyx());
15770         assertNull(calibrator.getEstimatedMyz());
15771         assertNull(calibrator.getEstimatedMzx());
15772         assertNull(calibrator.getEstimatedMzy());
15773         assertNull(calibrator.getEstimatedCovariance());
15774         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15775         assertNotNull(calibrator.getGroundTruthGravityNorm());
15776         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15777         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15778         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15779                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15780         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15781         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15782         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15783 
15784         // Force IllegalArgumentException
15785         calibrator = null;
15786         try {
15787             calibrator = new KnownGravityNormAccelerometerCalibrator(
15788                     -gravityNorm, measurements,
15789                     bx, by, bz, sx, sy, sz, mxy, mxz,
15790                     myx, myz, mzx, mzy);
15791             fail("IllegalArgumentException expected but not thrown");
15792         } catch (final IllegalArgumentException ignore) {
15793         }
15794         assertNull(calibrator);
15795     }
15796 
15797     @Test
15798     public void testConstructor128() throws WrongSizeException {
15799         final Collection<StandardDeviationBodyKinematics> measurements =
15800                 Collections.emptyList();
15801 
15802         final Matrix ba = generateBa();
15803         final double biasX = ba.getElementAtIndex(0);
15804         final double biasY = ba.getElementAtIndex(1);
15805         final double biasZ = ba.getElementAtIndex(2);
15806 
15807         final Acceleration bx = new Acceleration(biasX,
15808                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15809         final Acceleration by = new Acceleration(biasY,
15810                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15811         final Acceleration bz = new Acceleration(biasZ,
15812                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15813 
15814         final Matrix ma = generateMaCommonAxis();
15815         final double sx = ma.getElementAt(0, 0);
15816         final double sy = ma.getElementAt(1, 1);
15817         final double sz = ma.getElementAt(2, 2);
15818         final double mxy = ma.getElementAt(0, 1);
15819         final double mxz = ma.getElementAt(0, 2);
15820         final double myx = ma.getElementAt(1, 0);
15821         final double myz = ma.getElementAt(1, 2);
15822         final double mzx = ma.getElementAt(2, 0);
15823         final double mzy = ma.getElementAt(2, 1);
15824 
15825         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15826         final double latitude = Math.toRadians(
15827                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15828         final double longitude = Math.toRadians(
15829                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15830         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15831         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15832         final NEDVelocity nedVelocity = new NEDVelocity();
15833         final ECEFPosition ecefPosition = new ECEFPosition();
15834         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15835         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15836                 ecefPosition, ecefVelocity);
15837         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15838                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15839         final double gravityNorm = gravity.getNorm();
15840 
15841         KnownGravityNormAccelerometerCalibrator calibrator =
15842                 new KnownGravityNormAccelerometerCalibrator(
15843                         gravityNorm, measurements,
15844                         bx, by, bz, sx, sy, sz, mxy, mxz,
15845                         myx, myz, mzx, mzy, this);
15846 
15847         // check default values
15848         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15849         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15850         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15851         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15852         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15853         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15854         final Acceleration bx2 = new Acceleration(0.0,
15855                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15856         calibrator.getInitialBiasXAsAcceleration(bx2);
15857         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15858         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15859         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15860         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15861         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15862         final Acceleration by2 = new Acceleration(0.0,
15863                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15864         calibrator.getInitialBiasYAsAcceleration(by2);
15865         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15866         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15867         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15868         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15869         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15870         final Acceleration bz2 = new Acceleration(0.0,
15871                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15872         calibrator.getInitialBiasZAsAcceleration(bz2);
15873         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15874         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15875         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15876         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15877         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15878         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15879         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15880         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15881         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15882         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15883         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15884         final double[] bias1 = calibrator.getInitialBias();
15885         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15886         final double[] bias2 = new double[3];
15887         calibrator.getInitialBias(bias2);
15888         assertArrayEquals(bias1, bias2, 0.0);
15889         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15890         assertEquals(b1, ba);
15891         final Matrix b2 = new Matrix(3, 1);
15892         calibrator.getInitialBiasAsMatrix(b2);
15893         assertEquals(b1, b2);
15894         final Matrix ma1 = new Matrix(3, 3);
15895         ma1.setSubmatrix(0, 0,
15896                 2, 2,
15897                 new double[]{sx, myx, mzx,
15898                         mxy, sy, mzy,
15899                         mxz, myz, sz});
15900         assertEquals(calibrator.getInitialMa(), ma1);
15901         final Matrix ma2 = new Matrix(3, 3);
15902         calibrator.getInitialMa(ma2);
15903         assertEquals(ma1, ma2);
15904         assertSame(calibrator.getMeasurements(), measurements);
15905         assertFalse(calibrator.isCommonAxisUsed());
15906         assertSame(calibrator.getListener(), this);
15907         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15908         assertFalse(calibrator.isReady());
15909         assertFalse(calibrator.isRunning());
15910         assertNull(calibrator.getEstimatedBiases());
15911         assertFalse(calibrator.getEstimatedBiases(null));
15912         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15913         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15914         assertNull(calibrator.getEstimatedBiasFx());
15915         assertNull(calibrator.getEstimatedBiasFy());
15916         assertNull(calibrator.getEstimatedBiasFz());
15917         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15918         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15919         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15920         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15921         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15922         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15923         assertNull(calibrator.getEstimatedMa());
15924         assertNull(calibrator.getEstimatedSx());
15925         assertNull(calibrator.getEstimatedSy());
15926         assertNull(calibrator.getEstimatedSz());
15927         assertNull(calibrator.getEstimatedMxy());
15928         assertNull(calibrator.getEstimatedMxz());
15929         assertNull(calibrator.getEstimatedMyx());
15930         assertNull(calibrator.getEstimatedMyz());
15931         assertNull(calibrator.getEstimatedMzx());
15932         assertNull(calibrator.getEstimatedMzy());
15933         assertNull(calibrator.getEstimatedCovariance());
15934         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15935         assertNotNull(calibrator.getGroundTruthGravityNorm());
15936         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15937         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15938         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15939                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15940         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15941         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15942         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15943 
15944         // Force IllegalArgumentException
15945         calibrator = null;
15946         try {
15947             calibrator = new KnownGravityNormAccelerometerCalibrator(
15948                     -gravityNorm, measurements,
15949                     bx, by, bz, sx, sy, sz, mxy, mxz,
15950                     myx, myz, mzx, mzy, this);
15951             fail("IllegalArgumentException expected but not thrown");
15952         } catch (final IllegalArgumentException ignore) {
15953         }
15954         assertNull(calibrator);
15955     }
15956 
15957     @Test
15958     public void testConstructor129() throws WrongSizeException {
15959         final Matrix ba = generateBa();
15960         final double biasX = ba.getElementAtIndex(0);
15961         final double biasY = ba.getElementAtIndex(1);
15962         final double biasZ = ba.getElementAtIndex(2);
15963 
15964         final Acceleration bx = new Acceleration(biasX,
15965                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15966         final Acceleration by = new Acceleration(biasY,
15967                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15968         final Acceleration bz = new Acceleration(biasZ,
15969                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15970 
15971         final Matrix ma = generateMaCommonAxis();
15972         final double sx = ma.getElementAt(0, 0);
15973         final double sy = ma.getElementAt(1, 1);
15974         final double sz = ma.getElementAt(2, 2);
15975         final double mxy = ma.getElementAt(0, 1);
15976         final double mxz = ma.getElementAt(0, 2);
15977         final double myx = ma.getElementAt(1, 0);
15978         final double myz = ma.getElementAt(1, 2);
15979         final double mzx = ma.getElementAt(2, 0);
15980         final double mzy = ma.getElementAt(2, 1);
15981 
15982         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15983         final double latitude = Math.toRadians(
15984                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15985         final double longitude = Math.toRadians(
15986                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15987         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15988         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15989         final NEDVelocity nedVelocity = new NEDVelocity();
15990         final ECEFPosition ecefPosition = new ECEFPosition();
15991         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15992         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15993                 ecefPosition, ecefVelocity);
15994         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15995                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15996         final double gravityNorm = gravity.getNorm();
15997 
15998         KnownGravityNormAccelerometerCalibrator calibrator =
15999                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
16000                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
16001                         myx, myz, mzx, mzy);
16002 
16003         // check default values
16004         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16005         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16006         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16007         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16008         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16009         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16010         final Acceleration bx2 = new Acceleration(0.0,
16011                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16012         calibrator.getInitialBiasXAsAcceleration(bx2);
16013         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16014         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16015         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16016         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16017         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16018         final Acceleration by2 = new Acceleration(0.0,
16019                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16020         calibrator.getInitialBiasYAsAcceleration(by2);
16021         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16022         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16023         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16024         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16025         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16026         final Acceleration bz2 = new Acceleration(0.0,
16027                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16028         calibrator.getInitialBiasZAsAcceleration(bz2);
16029         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16030         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16031         assertEquals(calibrator.getInitialSx(), sx, 0.0);
16032         assertEquals(calibrator.getInitialSy(), sy, 0.0);
16033         assertEquals(calibrator.getInitialSz(), sz, 0.0);
16034         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
16035         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
16036         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
16037         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
16038         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
16039         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
16040         final double[] bias1 = calibrator.getInitialBias();
16041         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
16042         final double[] bias2 = new double[3];
16043         calibrator.getInitialBias(bias2);
16044         assertArrayEquals(bias1, bias2, 0.0);
16045         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16046         assertEquals(b1, ba);
16047         final Matrix b2 = new Matrix(3, 1);
16048         calibrator.getInitialBiasAsMatrix(b2);
16049         assertEquals(b1, b2);
16050         final Matrix ma1 = new Matrix(3, 3);
16051         ma1.setSubmatrix(0, 0,
16052                 2, 2,
16053                 new double[]{sx, myx, mzx,
16054                         mxy, sy, mzy,
16055                         mxz, myz, sz});
16056         assertEquals(calibrator.getInitialMa(), ma1);
16057         final Matrix ma2 = new Matrix(3, 3);
16058         calibrator.getInitialMa(ma2);
16059         assertEquals(ma1, ma2);
16060         assertNull(calibrator.getMeasurements());
16061         assertTrue(calibrator.isCommonAxisUsed());
16062         assertNull(calibrator.getListener());
16063         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16064         assertFalse(calibrator.isReady());
16065         assertFalse(calibrator.isRunning());
16066         assertNull(calibrator.getEstimatedBiases());
16067         assertFalse(calibrator.getEstimatedBiases(null));
16068         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16069         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16070         assertNull(calibrator.getEstimatedBiasFx());
16071         assertNull(calibrator.getEstimatedBiasFy());
16072         assertNull(calibrator.getEstimatedBiasFz());
16073         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16074         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16075         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16076         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16077         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16078         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16079         assertNull(calibrator.getEstimatedMa());
16080         assertNull(calibrator.getEstimatedSx());
16081         assertNull(calibrator.getEstimatedSy());
16082         assertNull(calibrator.getEstimatedSz());
16083         assertNull(calibrator.getEstimatedMxy());
16084         assertNull(calibrator.getEstimatedMxz());
16085         assertNull(calibrator.getEstimatedMyx());
16086         assertNull(calibrator.getEstimatedMyz());
16087         assertNull(calibrator.getEstimatedMzx());
16088         assertNull(calibrator.getEstimatedMzy());
16089         assertNull(calibrator.getEstimatedCovariance());
16090         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16091         assertNotNull(calibrator.getGroundTruthGravityNorm());
16092         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16093         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16094         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16095                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16096         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16097         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16098         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16099 
16100         // Force IllegalArgumentException
16101         calibrator = null;
16102         try {
16103             calibrator = new KnownGravityNormAccelerometerCalibrator(
16104                     -gravityNorm, true, bx, by, bz, sx, sy, sz, mxy, mxz,
16105                     myx, myz, mzx, mzy);
16106             fail("IllegalArgumentException expected but not thrown");
16107         } catch (final IllegalArgumentException ignore) {
16108         }
16109         assertNull(calibrator);
16110     }
16111 
16112     @Test
16113     public void testConstructor130() throws WrongSizeException {
16114         final Matrix ba = generateBa();
16115         final double biasX = ba.getElementAtIndex(0);
16116         final double biasY = ba.getElementAtIndex(1);
16117         final double biasZ = ba.getElementAtIndex(2);
16118 
16119         final Acceleration bx = new Acceleration(biasX,
16120                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16121         final Acceleration by = new Acceleration(biasY,
16122                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16123         final Acceleration bz = new Acceleration(biasZ,
16124                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16125 
16126         final Matrix ma = generateMaCommonAxis();
16127         final double sx = ma.getElementAt(0, 0);
16128         final double sy = ma.getElementAt(1, 1);
16129         final double sz = ma.getElementAt(2, 2);
16130         final double mxy = ma.getElementAt(0, 1);
16131         final double mxz = ma.getElementAt(0, 2);
16132         final double myx = ma.getElementAt(1, 0);
16133         final double myz = ma.getElementAt(1, 2);
16134         final double mzx = ma.getElementAt(2, 0);
16135         final double mzy = ma.getElementAt(2, 1);
16136 
16137         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16138         final double latitude = Math.toRadians(
16139                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16140         final double longitude = Math.toRadians(
16141                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16142         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16143         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16144         final NEDVelocity nedVelocity = new NEDVelocity();
16145         final ECEFPosition ecefPosition = new ECEFPosition();
16146         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16147         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16148                 ecefPosition, ecefVelocity);
16149         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16150                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16151         final double gravityNorm = gravity.getNorm();
16152 
16153         KnownGravityNormAccelerometerCalibrator calibrator =
16154                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
16155                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
16156                         myx, myz, mzx, mzy, this);
16157 
16158         // check default values
16159         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16160         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16161         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16162         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16163         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16164         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16165         final Acceleration bx2 = new Acceleration(0.0,
16166                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16167         calibrator.getInitialBiasXAsAcceleration(bx2);
16168         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16169         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16170         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16171         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16172         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16173         final Acceleration by2 = new Acceleration(0.0,
16174                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16175         calibrator.getInitialBiasYAsAcceleration(by2);
16176         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16177         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16178         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16179         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16180         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16181         final Acceleration bz2 = new Acceleration(0.0,
16182                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16183         calibrator.getInitialBiasZAsAcceleration(bz2);
16184         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16185         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16186         assertEquals(calibrator.getInitialSx(), sx, 0.0);
16187         assertEquals(calibrator.getInitialSy(), sy, 0.0);
16188         assertEquals(calibrator.getInitialSz(), sz, 0.0);
16189         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
16190         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
16191         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
16192         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
16193         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
16194         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
16195         final double[] bias1 = calibrator.getInitialBias();
16196         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
16197         final double[] bias2 = new double[3];
16198         calibrator.getInitialBias(bias2);
16199         assertArrayEquals(bias1, bias2, 0.0);
16200         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16201         assertEquals(b1, ba);
16202         final Matrix b2 = new Matrix(3, 1);
16203         calibrator.getInitialBiasAsMatrix(b2);
16204         assertEquals(b1, b2);
16205         final Matrix ma1 = new Matrix(3, 3);
16206         ma1.setSubmatrix(0, 0,
16207                 2, 2,
16208                 new double[]{sx, myx, mzx,
16209                         mxy, sy, mzy,
16210                         mxz, myz, sz});
16211         assertEquals(calibrator.getInitialMa(), ma1);
16212         final Matrix ma2 = new Matrix(3, 3);
16213         calibrator.getInitialMa(ma2);
16214         assertEquals(ma1, ma2);
16215         assertNull(calibrator.getMeasurements());
16216         assertTrue(calibrator.isCommonAxisUsed());
16217         assertSame(calibrator.getListener(), this);
16218         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16219         assertFalse(calibrator.isReady());
16220         assertFalse(calibrator.isRunning());
16221         assertNull(calibrator.getEstimatedBiases());
16222         assertFalse(calibrator.getEstimatedBiases(null));
16223         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16224         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16225         assertNull(calibrator.getEstimatedBiasFx());
16226         assertNull(calibrator.getEstimatedBiasFy());
16227         assertNull(calibrator.getEstimatedBiasFz());
16228         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16229         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16230         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16231         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16232         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16233         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16234         assertNull(calibrator.getEstimatedMa());
16235         assertNull(calibrator.getEstimatedSx());
16236         assertNull(calibrator.getEstimatedSy());
16237         assertNull(calibrator.getEstimatedSz());
16238         assertNull(calibrator.getEstimatedMxy());
16239         assertNull(calibrator.getEstimatedMxz());
16240         assertNull(calibrator.getEstimatedMyx());
16241         assertNull(calibrator.getEstimatedMyz());
16242         assertNull(calibrator.getEstimatedMzx());
16243         assertNull(calibrator.getEstimatedMzy());
16244         assertNull(calibrator.getEstimatedCovariance());
16245         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16246         assertNotNull(calibrator.getGroundTruthGravityNorm());
16247         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16248         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16249         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16250                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16251         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16252         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16253         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16254 
16255         // Force IllegalArgumentException
16256         calibrator = null;
16257         try {
16258             calibrator = new KnownGravityNormAccelerometerCalibrator(
16259                     -gravityNorm, true, bx, by, bz, sx, sy, sz, mxy, mxz,
16260                     myx, myz, mzx, mzy, this);
16261             fail("IllegalArgumentException expected but not thrown");
16262         } catch (final IllegalArgumentException ignore) {
16263         }
16264         assertNull(calibrator);
16265     }
16266 
16267     @Test
16268     public void testConstructor131() throws WrongSizeException {
16269         final Collection<StandardDeviationBodyKinematics> measurements =
16270                 Collections.emptyList();
16271 
16272         final Matrix ba = generateBa();
16273         final double biasX = ba.getElementAtIndex(0);
16274         final double biasY = ba.getElementAtIndex(1);
16275         final double biasZ = ba.getElementAtIndex(2);
16276 
16277         final Acceleration bx = new Acceleration(biasX,
16278                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16279         final Acceleration by = new Acceleration(biasY,
16280                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16281         final Acceleration bz = new Acceleration(biasZ,
16282                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16283 
16284         final Matrix ma = generateMaCommonAxis();
16285         final double sx = ma.getElementAt(0, 0);
16286         final double sy = ma.getElementAt(1, 1);
16287         final double sz = ma.getElementAt(2, 2);
16288         final double mxy = ma.getElementAt(0, 1);
16289         final double mxz = ma.getElementAt(0, 2);
16290         final double myx = ma.getElementAt(1, 0);
16291         final double myz = ma.getElementAt(1, 2);
16292         final double mzx = ma.getElementAt(2, 0);
16293         final double mzy = ma.getElementAt(2, 1);
16294 
16295         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16296         final double latitude = Math.toRadians(
16297                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16298         final double longitude = Math.toRadians(
16299                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16300         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16301         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16302         final NEDVelocity nedVelocity = new NEDVelocity();
16303         final ECEFPosition ecefPosition = new ECEFPosition();
16304         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16305         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16306                 ecefPosition, ecefVelocity);
16307         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16308                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16309         final double gravityNorm = gravity.getNorm();
16310 
16311         KnownGravityNormAccelerometerCalibrator calibrator =
16312                 new KnownGravityNormAccelerometerCalibrator(
16313                         gravityNorm, measurements,
16314                         true, bx, by, bz, sx, sy, sz,
16315                         mxy, mxz, myx, myz, mzx, mzy);
16316 
16317         // check default values
16318         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16319         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16320         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16321         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16322         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16323         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16324         final Acceleration bx2 = new Acceleration(0.0,
16325                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16326         calibrator.getInitialBiasXAsAcceleration(bx2);
16327         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16328         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16329         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16330         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16331         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16332         final Acceleration by2 = new Acceleration(0.0,
16333                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16334         calibrator.getInitialBiasYAsAcceleration(by2);
16335         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16336         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16337         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16338         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16339         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16340         final Acceleration bz2 = new Acceleration(0.0,
16341                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16342         calibrator.getInitialBiasZAsAcceleration(bz2);
16343         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16344         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16345         assertEquals(calibrator.getInitialSx(), sx, 0.0);
16346         assertEquals(calibrator.getInitialSy(), sy, 0.0);
16347         assertEquals(calibrator.getInitialSz(), sz, 0.0);
16348         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
16349         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
16350         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
16351         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
16352         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
16353         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
16354         final double[] bias1 = calibrator.getInitialBias();
16355         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
16356         final double[] bias2 = new double[3];
16357         calibrator.getInitialBias(bias2);
16358         assertArrayEquals(bias1, bias2, 0.0);
16359         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16360         assertEquals(b1, ba);
16361         final Matrix b2 = new Matrix(3, 1);
16362         calibrator.getInitialBiasAsMatrix(b2);
16363         assertEquals(b1, b2);
16364         final Matrix ma1 = new Matrix(3, 3);
16365         ma1.setSubmatrix(0, 0,
16366                 2, 2,
16367                 new double[]{sx, myx, mzx,
16368                         mxy, sy, mzy,
16369                         mxz, myz, sz});
16370         assertEquals(calibrator.getInitialMa(), ma1);
16371         final Matrix ma2 = new Matrix(3, 3);
16372         calibrator.getInitialMa(ma2);
16373         assertEquals(ma1, ma2);
16374         assertSame(calibrator.getMeasurements(), measurements);
16375         assertTrue(calibrator.isCommonAxisUsed());
16376         assertNull(calibrator.getListener());
16377         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16378         assertFalse(calibrator.isReady());
16379         assertFalse(calibrator.isRunning());
16380         assertNull(calibrator.getEstimatedBiases());
16381         assertFalse(calibrator.getEstimatedBiases(null));
16382         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16383         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16384         assertNull(calibrator.getEstimatedBiasFx());
16385         assertNull(calibrator.getEstimatedBiasFy());
16386         assertNull(calibrator.getEstimatedBiasFz());
16387         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16388         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16389         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16390         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16391         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16392         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16393         assertNull(calibrator.getEstimatedMa());
16394         assertNull(calibrator.getEstimatedSx());
16395         assertNull(calibrator.getEstimatedSy());
16396         assertNull(calibrator.getEstimatedSz());
16397         assertNull(calibrator.getEstimatedMxy());
16398         assertNull(calibrator.getEstimatedMxz());
16399         assertNull(calibrator.getEstimatedMyx());
16400         assertNull(calibrator.getEstimatedMyz());
16401         assertNull(calibrator.getEstimatedMzx());
16402         assertNull(calibrator.getEstimatedMzy());
16403         assertNull(calibrator.getEstimatedCovariance());
16404         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16405         assertNotNull(calibrator.getGroundTruthGravityNorm());
16406         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16407         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16408         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16409                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16410         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16411         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16412         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16413 
16414         // Force IllegalArgumentException
16415         calibrator = null;
16416         try {
16417             calibrator = new KnownGravityNormAccelerometerCalibrator(
16418                     -gravityNorm, measurements,
16419                     true, bx, by, bz, sx, sy, sz,
16420                     mxy, mxz, myx, myz, mzx, mzy);
16421             fail("IllegalArgumentException expected but not thrown");
16422         } catch (final IllegalArgumentException ignore) {
16423         }
16424         assertNull(calibrator);
16425     }
16426 
16427     @Test
16428     public void testConstructor132() throws WrongSizeException {
16429         final Collection<StandardDeviationBodyKinematics> measurements =
16430                 Collections.emptyList();
16431 
16432         final Matrix ba = generateBa();
16433         final double biasX = ba.getElementAtIndex(0);
16434         final double biasY = ba.getElementAtIndex(1);
16435         final double biasZ = ba.getElementAtIndex(2);
16436 
16437         final Acceleration bx = new Acceleration(biasX,
16438                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16439         final Acceleration by = new Acceleration(biasY,
16440                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16441         final Acceleration bz = new Acceleration(biasZ,
16442                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16443 
16444         final Matrix ma = generateMaCommonAxis();
16445         final double sx = ma.getElementAt(0, 0);
16446         final double sy = ma.getElementAt(1, 1);
16447         final double sz = ma.getElementAt(2, 2);
16448         final double mxy = ma.getElementAt(0, 1);
16449         final double mxz = ma.getElementAt(0, 2);
16450         final double myx = ma.getElementAt(1, 0);
16451         final double myz = ma.getElementAt(1, 2);
16452         final double mzx = ma.getElementAt(2, 0);
16453         final double mzy = ma.getElementAt(2, 1);
16454 
16455         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16456         final double latitude = Math.toRadians(
16457                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16458         final double longitude = Math.toRadians(
16459                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16460         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16461         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16462         final NEDVelocity nedVelocity = new NEDVelocity();
16463         final ECEFPosition ecefPosition = new ECEFPosition();
16464         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16465         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16466                 ecefPosition, ecefVelocity);
16467         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16468                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16469         final double gravityNorm = gravity.getNorm();
16470 
16471         KnownGravityNormAccelerometerCalibrator calibrator =
16472                 new KnownGravityNormAccelerometerCalibrator(
16473                         gravityNorm, measurements,
16474                         true, bx, by, bz, sx, sy, sz,
16475                         mxy, mxz, myx, myz, mzx, mzy, this);
16476 
16477         // check default values
16478         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16479         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16480         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16481         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16482         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16483         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16484         final Acceleration bx2 = new Acceleration(0.0,
16485                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16486         calibrator.getInitialBiasXAsAcceleration(bx2);
16487         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16488         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16489         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16490         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16491         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16492         final Acceleration by2 = new Acceleration(0.0,
16493                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16494         calibrator.getInitialBiasYAsAcceleration(by2);
16495         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16496         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16497         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16498         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16499         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16500         final Acceleration bz2 = new Acceleration(0.0,
16501                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16502         calibrator.getInitialBiasZAsAcceleration(bz2);
16503         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16504         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16505         assertEquals(calibrator.getInitialSx(), sx, 0.0);
16506         assertEquals(calibrator.getInitialSy(), sy, 0.0);
16507         assertEquals(calibrator.getInitialSz(), sz, 0.0);
16508         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
16509         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
16510         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
16511         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
16512         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
16513         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
16514         final double[] bias1 = calibrator.getInitialBias();
16515         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
16516         final double[] bias2 = new double[3];
16517         calibrator.getInitialBias(bias2);
16518         assertArrayEquals(bias1, bias2, 0.0);
16519         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16520         assertEquals(b1, ba);
16521         final Matrix b2 = new Matrix(3, 1);
16522         calibrator.getInitialBiasAsMatrix(b2);
16523         assertEquals(b1, b2);
16524         final Matrix ma1 = new Matrix(3, 3);
16525         ma1.setSubmatrix(0, 0,
16526                 2, 2,
16527                 new double[]{sx, myx, mzx,
16528                         mxy, sy, mzy,
16529                         mxz, myz, sz});
16530         assertEquals(calibrator.getInitialMa(), ma1);
16531         final Matrix ma2 = new Matrix(3, 3);
16532         calibrator.getInitialMa(ma2);
16533         assertEquals(ma1, ma2);
16534         assertSame(calibrator.getMeasurements(), measurements);
16535         assertTrue(calibrator.isCommonAxisUsed());
16536         assertSame(calibrator.getListener(), this);
16537         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16538         assertFalse(calibrator.isReady());
16539         assertFalse(calibrator.isRunning());
16540         assertNull(calibrator.getEstimatedBiases());
16541         assertFalse(calibrator.getEstimatedBiases(null));
16542         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16543         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16544         assertNull(calibrator.getEstimatedBiasFx());
16545         assertNull(calibrator.getEstimatedBiasFy());
16546         assertNull(calibrator.getEstimatedBiasFz());
16547         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16548         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16549         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16550         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16551         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16552         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16553         assertNull(calibrator.getEstimatedMa());
16554         assertNull(calibrator.getEstimatedSx());
16555         assertNull(calibrator.getEstimatedSy());
16556         assertNull(calibrator.getEstimatedSz());
16557         assertNull(calibrator.getEstimatedMxy());
16558         assertNull(calibrator.getEstimatedMxz());
16559         assertNull(calibrator.getEstimatedMyx());
16560         assertNull(calibrator.getEstimatedMyz());
16561         assertNull(calibrator.getEstimatedMzx());
16562         assertNull(calibrator.getEstimatedMzy());
16563         assertNull(calibrator.getEstimatedCovariance());
16564         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16565         assertNotNull(calibrator.getGroundTruthGravityNorm());
16566         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16567         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16568         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16569                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16570         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16571         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16572         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16573 
16574         // Force IllegalArgumentException
16575         calibrator = null;
16576         try {
16577             calibrator = new KnownGravityNormAccelerometerCalibrator(
16578                     -gravityNorm, measurements,
16579                     true, bx, by, bz, sx, sy, sz,
16580                     mxy, mxz, myx, myz, mzx, mzy, this);
16581             fail("IllegalArgumentException expected but not thrown");
16582         } catch (final IllegalArgumentException ignore) {
16583         }
16584         assertNull(calibrator);
16585     }
16586 
16587     @Test
16588     public void testConstructor133() throws WrongSizeException {
16589         final Matrix ba = generateBa();
16590         final double[] bias = ba.getBuffer();
16591         final double biasX = ba.getElementAtIndex(0);
16592         final double biasY = ba.getElementAtIndex(1);
16593         final double biasZ = ba.getElementAtIndex(2);
16594 
16595         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16596         final double latitude = Math.toRadians(
16597                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16598         final double longitude = Math.toRadians(
16599                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16600         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16601         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16602         final NEDVelocity nedVelocity = new NEDVelocity();
16603         final ECEFPosition ecefPosition = new ECEFPosition();
16604         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16605         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16606                 ecefPosition, ecefVelocity);
16607         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16608                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16609         final double gravityNorm = gravity.getNorm();
16610 
16611         KnownGravityNormAccelerometerCalibrator calibrator =
16612                 new KnownGravityNormAccelerometerCalibrator(
16613                         gravityNorm, bias);
16614 
16615         // check default values
16616         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16617         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16618         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16619         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16620         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16621         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16622         final Acceleration bx2 = new Acceleration(0.0,
16623                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16624         calibrator.getInitialBiasXAsAcceleration(bx2);
16625         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16626         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16627         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16628         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16629         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16630         final Acceleration by2 = new Acceleration(0.0,
16631                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16632         calibrator.getInitialBiasYAsAcceleration(by2);
16633         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16634         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16635         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16636         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16637         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16638         final Acceleration bz2 = new Acceleration(0.0,
16639                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16640         calibrator.getInitialBiasZAsAcceleration(bz2);
16641         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16642         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16643         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16644         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16645         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16646         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16647         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16648         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16649         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16650         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16651         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16652         final double[] bias1 = calibrator.getInitialBias();
16653         assertArrayEquals(bias1, bias, 0.0);
16654         final double[] bias2 = new double[3];
16655         calibrator.getInitialBias(bias2);
16656         assertArrayEquals(bias1, bias2, 0.0);
16657         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16658         assertEquals(b1, ba);
16659         final Matrix b2 = new Matrix(3, 1);
16660         calibrator.getInitialBiasAsMatrix(b2);
16661         assertEquals(b1, b2);
16662         final Matrix ma1 = calibrator.getInitialMa();
16663         assertEquals(ma1, new Matrix(3, 3));
16664         final Matrix ma2 = new Matrix(3, 3);
16665         calibrator.getInitialMa(ma2);
16666         assertEquals(ma1, ma2);
16667         assertNull(calibrator.getMeasurements());
16668         assertFalse(calibrator.isCommonAxisUsed());
16669         assertNull(calibrator.getListener());
16670         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16671         assertFalse(calibrator.isReady());
16672         assertFalse(calibrator.isRunning());
16673         assertNull(calibrator.getEstimatedBiases());
16674         assertFalse(calibrator.getEstimatedBiases(null));
16675         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16676         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16677         assertNull(calibrator.getEstimatedBiasFx());
16678         assertNull(calibrator.getEstimatedBiasFy());
16679         assertNull(calibrator.getEstimatedBiasFz());
16680         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16681         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16682         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16683         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16684         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16685         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16686         assertNull(calibrator.getEstimatedMa());
16687         assertNull(calibrator.getEstimatedSx());
16688         assertNull(calibrator.getEstimatedSy());
16689         assertNull(calibrator.getEstimatedSz());
16690         assertNull(calibrator.getEstimatedMxy());
16691         assertNull(calibrator.getEstimatedMxz());
16692         assertNull(calibrator.getEstimatedMyx());
16693         assertNull(calibrator.getEstimatedMyz());
16694         assertNull(calibrator.getEstimatedMzx());
16695         assertNull(calibrator.getEstimatedMzy());
16696         assertNull(calibrator.getEstimatedCovariance());
16697         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16698         assertNotNull(calibrator.getGroundTruthGravityNorm());
16699         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16700         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16701         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16702                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16703         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16704         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16705         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16706 
16707         // Force IllegalArgumentException
16708         calibrator = null;
16709         try {
16710             calibrator = new KnownGravityNormAccelerometerCalibrator(
16711                     -gravityNorm, bias);
16712             fail("IllegalArgumentException expected but not thrown");
16713         } catch (final IllegalArgumentException ignore) {
16714         }
16715         try {
16716             calibrator = new KnownGravityNormAccelerometerCalibrator(gravityNorm,
16717                     new double[1]);
16718             fail("IllegalArgumentException expected but not thrown");
16719         } catch (final IllegalArgumentException ignore) {
16720         }
16721         assertNull(calibrator);
16722     }
16723 
16724     @Test
16725     public void testConstructor134() throws WrongSizeException {
16726         final Matrix ba = generateBa();
16727         final double[] bias = ba.getBuffer();
16728         final double biasX = ba.getElementAtIndex(0);
16729         final double biasY = ba.getElementAtIndex(1);
16730         final double biasZ = ba.getElementAtIndex(2);
16731 
16732         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16733         final double latitude = Math.toRadians(
16734                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16735         final double longitude = Math.toRadians(
16736                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16737         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16738         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16739         final NEDVelocity nedVelocity = new NEDVelocity();
16740         final ECEFPosition ecefPosition = new ECEFPosition();
16741         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16742         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16743                 ecefPosition, ecefVelocity);
16744         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16745                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16746         final double gravityNorm = gravity.getNorm();
16747 
16748         KnownGravityNormAccelerometerCalibrator calibrator =
16749                 new KnownGravityNormAccelerometerCalibrator(
16750                         gravityNorm, bias, this);
16751 
16752         // check default values
16753         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16754         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16755         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16756         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16757         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16758         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16759         final Acceleration bx2 = new Acceleration(0.0,
16760                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16761         calibrator.getInitialBiasXAsAcceleration(bx2);
16762         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16763         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16764         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16765         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16766         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16767         final Acceleration by2 = new Acceleration(0.0,
16768                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16769         calibrator.getInitialBiasYAsAcceleration(by2);
16770         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16771         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16772         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16773         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16774         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16775         final Acceleration bz2 = new Acceleration(0.0,
16776                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16777         calibrator.getInitialBiasZAsAcceleration(bz2);
16778         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16779         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16780         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16781         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16782         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16783         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16784         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16785         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16786         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16787         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16788         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16789         final double[] bias1 = calibrator.getInitialBias();
16790         assertArrayEquals(bias1, bias, 0.0);
16791         final double[] bias2 = new double[3];
16792         calibrator.getInitialBias(bias2);
16793         assertArrayEquals(bias1, bias2, 0.0);
16794         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16795         assertEquals(b1, ba);
16796         final Matrix b2 = new Matrix(3, 1);
16797         calibrator.getInitialBiasAsMatrix(b2);
16798         assertEquals(b1, b2);
16799         final Matrix ma1 = calibrator.getInitialMa();
16800         assertEquals(ma1, new Matrix(3, 3));
16801         final Matrix ma2 = new Matrix(3, 3);
16802         calibrator.getInitialMa(ma2);
16803         assertEquals(ma1, ma2);
16804         assertNull(calibrator.getMeasurements());
16805         assertFalse(calibrator.isCommonAxisUsed());
16806         assertSame(calibrator.getListener(), this);
16807         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16808         assertFalse(calibrator.isReady());
16809         assertFalse(calibrator.isRunning());
16810         assertNull(calibrator.getEstimatedBiases());
16811         assertFalse(calibrator.getEstimatedBiases(null));
16812         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16813         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16814         assertNull(calibrator.getEstimatedBiasFx());
16815         assertNull(calibrator.getEstimatedBiasFy());
16816         assertNull(calibrator.getEstimatedBiasFz());
16817         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16818         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16819         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16820         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16821         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16822         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16823         assertNull(calibrator.getEstimatedMa());
16824         assertNull(calibrator.getEstimatedSx());
16825         assertNull(calibrator.getEstimatedSy());
16826         assertNull(calibrator.getEstimatedSz());
16827         assertNull(calibrator.getEstimatedMxy());
16828         assertNull(calibrator.getEstimatedMxz());
16829         assertNull(calibrator.getEstimatedMyx());
16830         assertNull(calibrator.getEstimatedMyz());
16831         assertNull(calibrator.getEstimatedMzx());
16832         assertNull(calibrator.getEstimatedMzy());
16833         assertNull(calibrator.getEstimatedCovariance());
16834         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16835         assertNotNull(calibrator.getGroundTruthGravityNorm());
16836         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16837         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16838         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16839                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16840         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16841         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16842         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16843 
16844         // Force IllegalArgumentException
16845         calibrator = null;
16846         try {
16847             calibrator = new KnownGravityNormAccelerometerCalibrator(
16848                     -gravityNorm, bias, this);
16849             fail("IllegalArgumentException expected but not thrown");
16850         } catch (final IllegalArgumentException ignore) {
16851         }
16852         try {
16853             calibrator = new KnownGravityNormAccelerometerCalibrator(
16854                     gravityNorm, new double[1], this);
16855             fail("IllegalArgumentException expected but not thrown");
16856         } catch (final IllegalArgumentException ignore) {
16857         }
16858         assertNull(calibrator);
16859     }
16860 
16861     @Test
16862     public void testConstructor135() throws WrongSizeException {
16863         final Collection<StandardDeviationBodyKinematics> measurements =
16864                 Collections.emptyList();
16865 
16866         final Matrix ba = generateBa();
16867         final double[] bias = ba.getBuffer();
16868         final double biasX = ba.getElementAtIndex(0);
16869         final double biasY = ba.getElementAtIndex(1);
16870         final double biasZ = ba.getElementAtIndex(2);
16871 
16872         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16873         final double latitude = Math.toRadians(
16874                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16875         final double longitude = Math.toRadians(
16876                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16877         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16878         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16879         final NEDVelocity nedVelocity = new NEDVelocity();
16880         final ECEFPosition ecefPosition = new ECEFPosition();
16881         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16882         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16883                 ecefPosition, ecefVelocity);
16884         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16885                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16886         final double gravityNorm = gravity.getNorm();
16887 
16888         KnownGravityNormAccelerometerCalibrator calibrator =
16889                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
16890                         measurements, bias);
16891 
16892         // check default values
16893         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16894         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16895         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16896         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16897         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16898         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16899         final Acceleration bx2 = new Acceleration(0.0,
16900                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16901         calibrator.getInitialBiasXAsAcceleration(bx2);
16902         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16903         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16904         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16905         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16906         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16907         final Acceleration by2 = new Acceleration(0.0,
16908                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16909         calibrator.getInitialBiasYAsAcceleration(by2);
16910         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16911         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16912         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16913         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16914         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16915         final Acceleration bz2 = new Acceleration(0.0,
16916                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16917         calibrator.getInitialBiasZAsAcceleration(bz2);
16918         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16919         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16920         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16921         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16922         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16923         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16924         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16925         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16926         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16927         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16928         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16929         final double[] bias1 = calibrator.getInitialBias();
16930         assertArrayEquals(bias1, bias, 0.0);
16931         final double[] bias2 = new double[3];
16932         calibrator.getInitialBias(bias2);
16933         assertArrayEquals(bias1, bias2, 0.0);
16934         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16935         assertEquals(b1, ba);
16936         final Matrix b2 = new Matrix(3, 1);
16937         calibrator.getInitialBiasAsMatrix(b2);
16938         assertEquals(b1, b2);
16939         final Matrix ma1 = calibrator.getInitialMa();
16940         assertEquals(ma1, new Matrix(3, 3));
16941         final Matrix ma2 = new Matrix(3, 3);
16942         calibrator.getInitialMa(ma2);
16943         assertEquals(ma1, ma2);
16944         assertSame(calibrator.getMeasurements(), measurements);
16945         assertFalse(calibrator.isCommonAxisUsed());
16946         assertNull(calibrator.getListener());
16947         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16948         assertFalse(calibrator.isReady());
16949         assertFalse(calibrator.isRunning());
16950         assertNull(calibrator.getEstimatedBiases());
16951         assertFalse(calibrator.getEstimatedBiases(null));
16952         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16953         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16954         assertNull(calibrator.getEstimatedBiasFx());
16955         assertNull(calibrator.getEstimatedBiasFy());
16956         assertNull(calibrator.getEstimatedBiasFz());
16957         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16958         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16959         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16960         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16961         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16962         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16963         assertNull(calibrator.getEstimatedMa());
16964         assertNull(calibrator.getEstimatedSx());
16965         assertNull(calibrator.getEstimatedSy());
16966         assertNull(calibrator.getEstimatedSz());
16967         assertNull(calibrator.getEstimatedMxy());
16968         assertNull(calibrator.getEstimatedMxz());
16969         assertNull(calibrator.getEstimatedMyx());
16970         assertNull(calibrator.getEstimatedMyz());
16971         assertNull(calibrator.getEstimatedMzx());
16972         assertNull(calibrator.getEstimatedMzy());
16973         assertNull(calibrator.getEstimatedCovariance());
16974         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16975         assertNotNull(calibrator.getGroundTruthGravityNorm());
16976         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16977         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16978         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16979                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16980         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16981         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16982         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16983 
16984         // Force IllegalArgumentException
16985         calibrator = null;
16986         try {
16987             calibrator = new KnownGravityNormAccelerometerCalibrator(
16988                     -gravityNorm, measurements, bias);
16989             fail("IllegalArgumentException expected but not thrown");
16990         } catch (final IllegalArgumentException ignore) {
16991         }
16992         try {
16993             calibrator = new KnownGravityNormAccelerometerCalibrator(
16994                     gravityNorm, measurements, new double[1]);
16995             fail("IllegalArgumentException expected but not thrown");
16996         } catch (final IllegalArgumentException ignore) {
16997         }
16998         assertNull(calibrator);
16999     }
17000 
17001     @Test
17002     public void testConstructor136() throws WrongSizeException {
17003         final Collection<StandardDeviationBodyKinematics> measurements =
17004                 Collections.emptyList();
17005 
17006         final Matrix ba = generateBa();
17007         final double[] bias = ba.getBuffer();
17008         final double biasX = ba.getElementAtIndex(0);
17009         final double biasY = ba.getElementAtIndex(1);
17010         final double biasZ = ba.getElementAtIndex(2);
17011 
17012         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17013         final double latitude = Math.toRadians(
17014                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17015         final double longitude = Math.toRadians(
17016                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17017         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17018         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17019         final NEDVelocity nedVelocity = new NEDVelocity();
17020         final ECEFPosition ecefPosition = new ECEFPosition();
17021         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17022         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17023                 ecefPosition, ecefVelocity);
17024         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17025                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17026         final double gravityNorm = gravity.getNorm();
17027 
17028         KnownGravityNormAccelerometerCalibrator calibrator =
17029                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
17030                         measurements, bias, this);
17031 
17032         // check default values
17033         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17034         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17035         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17036         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17037         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17038         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17039         final Acceleration bx2 = new Acceleration(0.0,
17040                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17041         calibrator.getInitialBiasXAsAcceleration(bx2);
17042         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17043         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17044         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17045         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17046         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17047         final Acceleration by2 = new Acceleration(0.0,
17048                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17049         calibrator.getInitialBiasYAsAcceleration(by2);
17050         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17051         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17052         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17053         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17054         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17055         final Acceleration bz2 = new Acceleration(0.0,
17056                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17057         calibrator.getInitialBiasZAsAcceleration(bz2);
17058         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17059         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17060         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17061         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17062         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17063         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17064         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17065         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17066         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17067         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17068         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17069         final double[] bias1 = calibrator.getInitialBias();
17070         assertArrayEquals(bias1, bias, 0.0);
17071         final double[] bias2 = new double[3];
17072         calibrator.getInitialBias(bias2);
17073         assertArrayEquals(bias1, bias2, 0.0);
17074         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17075         assertEquals(b1, ba);
17076         final Matrix b2 = new Matrix(3, 1);
17077         calibrator.getInitialBiasAsMatrix(b2);
17078         assertEquals(b1, b2);
17079         final Matrix ma1 = calibrator.getInitialMa();
17080         assertEquals(ma1, new Matrix(3, 3));
17081         final Matrix ma2 = new Matrix(3, 3);
17082         calibrator.getInitialMa(ma2);
17083         assertEquals(ma1, ma2);
17084         assertSame(calibrator.getMeasurements(), measurements);
17085         assertFalse(calibrator.isCommonAxisUsed());
17086         assertSame(calibrator.getListener(), this);
17087         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17088         assertFalse(calibrator.isReady());
17089         assertFalse(calibrator.isRunning());
17090         assertNull(calibrator.getEstimatedBiases());
17091         assertFalse(calibrator.getEstimatedBiases(null));
17092         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17093         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17094         assertNull(calibrator.getEstimatedBiasFx());
17095         assertNull(calibrator.getEstimatedBiasFy());
17096         assertNull(calibrator.getEstimatedBiasFz());
17097         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17098         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17099         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17100         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17101         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17102         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17103         assertNull(calibrator.getEstimatedMa());
17104         assertNull(calibrator.getEstimatedSx());
17105         assertNull(calibrator.getEstimatedSy());
17106         assertNull(calibrator.getEstimatedSz());
17107         assertNull(calibrator.getEstimatedMxy());
17108         assertNull(calibrator.getEstimatedMxz());
17109         assertNull(calibrator.getEstimatedMyx());
17110         assertNull(calibrator.getEstimatedMyz());
17111         assertNull(calibrator.getEstimatedMzx());
17112         assertNull(calibrator.getEstimatedMzy());
17113         assertNull(calibrator.getEstimatedCovariance());
17114         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17115         assertNotNull(calibrator.getGroundTruthGravityNorm());
17116         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17117         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17118         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17119                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17120         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17121         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17122         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17123 
17124         // Force IllegalArgumentException
17125         calibrator = null;
17126         try {
17127             calibrator = new KnownGravityNormAccelerometerCalibrator(
17128                     -gravityNorm, measurements, bias, this);
17129             fail("IllegalArgumentException expected but not thrown");
17130         } catch (final IllegalArgumentException ignore) {
17131         }
17132         try {
17133             calibrator = new KnownGravityNormAccelerometerCalibrator(
17134                     gravityNorm, measurements, new double[1], this);
17135             fail("IllegalArgumentException expected but not thrown");
17136         } catch (final IllegalArgumentException ignore) {
17137         }
17138         assertNull(calibrator);
17139     }
17140 
17141     @Test
17142     public void testConstructor137() throws WrongSizeException {
17143         final Matrix ba = generateBa();
17144         final double[] bias = ba.getBuffer();
17145         final double biasX = ba.getElementAtIndex(0);
17146         final double biasY = ba.getElementAtIndex(1);
17147         final double biasZ = ba.getElementAtIndex(2);
17148 
17149         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17150         final double latitude = Math.toRadians(
17151                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17152         final double longitude = Math.toRadians(
17153                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17154         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17155         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17156         final NEDVelocity nedVelocity = new NEDVelocity();
17157         final ECEFPosition ecefPosition = new ECEFPosition();
17158         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17159         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17160                 ecefPosition, ecefVelocity);
17161         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17162                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17163         final double gravityNorm = gravity.getNorm();
17164 
17165         KnownGravityNormAccelerometerCalibrator calibrator =
17166                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
17167                         true, bias);
17168 
17169         // check default values
17170         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17171         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17172         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17173         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17174         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17175         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17176         final Acceleration bx2 = new Acceleration(0.0,
17177                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17178         calibrator.getInitialBiasXAsAcceleration(bx2);
17179         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17180         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17181         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17182         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17183         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17184         final Acceleration by2 = new Acceleration(0.0,
17185                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17186         calibrator.getInitialBiasYAsAcceleration(by2);
17187         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17188         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17189         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17190         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17191         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17192         final Acceleration bz2 = new Acceleration(0.0,
17193                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17194         calibrator.getInitialBiasZAsAcceleration(bz2);
17195         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17196         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17197         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17198         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17199         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17200         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17201         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17202         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17203         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17204         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17205         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17206         final double[] bias1 = calibrator.getInitialBias();
17207         assertArrayEquals(bias1, bias, 0.0);
17208         final double[] bias2 = new double[3];
17209         calibrator.getInitialBias(bias2);
17210         assertArrayEquals(bias1, bias2, 0.0);
17211         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17212         assertEquals(b1, ba);
17213         final Matrix b2 = new Matrix(3, 1);
17214         calibrator.getInitialBiasAsMatrix(b2);
17215         assertEquals(b1, b2);
17216         final Matrix ma1 = calibrator.getInitialMa();
17217         assertEquals(ma1, new Matrix(3, 3));
17218         final Matrix ma2 = new Matrix(3, 3);
17219         calibrator.getInitialMa(ma2);
17220         assertEquals(ma1, ma2);
17221         assertNull(calibrator.getMeasurements());
17222         assertTrue(calibrator.isCommonAxisUsed());
17223         assertNull(calibrator.getListener());
17224         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17225         assertFalse(calibrator.isReady());
17226         assertFalse(calibrator.isRunning());
17227         assertNull(calibrator.getEstimatedBiases());
17228         assertFalse(calibrator.getEstimatedBiases(null));
17229         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17230         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17231         assertNull(calibrator.getEstimatedBiasFx());
17232         assertNull(calibrator.getEstimatedBiasFy());
17233         assertNull(calibrator.getEstimatedBiasFz());
17234         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17235         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17236         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17237         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17238         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17239         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17240         assertNull(calibrator.getEstimatedMa());
17241         assertNull(calibrator.getEstimatedSx());
17242         assertNull(calibrator.getEstimatedSy());
17243         assertNull(calibrator.getEstimatedSz());
17244         assertNull(calibrator.getEstimatedMxy());
17245         assertNull(calibrator.getEstimatedMxz());
17246         assertNull(calibrator.getEstimatedMyx());
17247         assertNull(calibrator.getEstimatedMyz());
17248         assertNull(calibrator.getEstimatedMzx());
17249         assertNull(calibrator.getEstimatedMzy());
17250         assertNull(calibrator.getEstimatedCovariance());
17251         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17252         assertNotNull(calibrator.getGroundTruthGravityNorm());
17253         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17254         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17255         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17256                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17257         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17258         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17259         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17260 
17261         // Force IllegalArgumentException
17262         calibrator = null;
17263         try {
17264             calibrator = new KnownGravityNormAccelerometerCalibrator(
17265                     -gravityNorm, true, bias);
17266             fail("IllegalArgumentException expected but not thrown");
17267         } catch (final IllegalArgumentException ignore) {
17268         }
17269         try {
17270             calibrator = new KnownGravityNormAccelerometerCalibrator(
17271                     gravityNorm, true, new double[1]);
17272             fail("IllegalArgumentException expected but not thrown");
17273         } catch (final IllegalArgumentException ignore) {
17274         }
17275         assertNull(calibrator);
17276     }
17277 
17278     @Test
17279     public void testConstructor138() throws WrongSizeException {
17280         final Matrix ba = generateBa();
17281         final double[] bias = ba.getBuffer();
17282         final double biasX = ba.getElementAtIndex(0);
17283         final double biasY = ba.getElementAtIndex(1);
17284         final double biasZ = ba.getElementAtIndex(2);
17285 
17286         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17287         final double latitude = Math.toRadians(
17288                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17289         final double longitude = Math.toRadians(
17290                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17291         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17292         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17293         final NEDVelocity nedVelocity = new NEDVelocity();
17294         final ECEFPosition ecefPosition = new ECEFPosition();
17295         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17296         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17297                 ecefPosition, ecefVelocity);
17298         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17299                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17300         final double gravityNorm = gravity.getNorm();
17301 
17302         KnownGravityNormAccelerometerCalibrator calibrator =
17303                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
17304                         true, bias, this);
17305 
17306         // check default values
17307         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17308         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17309         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17310         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17311         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17312         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17313         final Acceleration bx2 = new Acceleration(0.0,
17314                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17315         calibrator.getInitialBiasXAsAcceleration(bx2);
17316         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17317         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17318         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17319         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17320         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17321         final Acceleration by2 = new Acceleration(0.0,
17322                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17323         calibrator.getInitialBiasYAsAcceleration(by2);
17324         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17325         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17326         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17327         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17328         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17329         final Acceleration bz2 = new Acceleration(0.0,
17330                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17331         calibrator.getInitialBiasZAsAcceleration(bz2);
17332         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17333         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17334         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17335         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17336         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17337         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17338         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17339         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17340         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17341         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17342         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17343         final double[] bias1 = calibrator.getInitialBias();
17344         assertArrayEquals(bias1, bias, 0.0);
17345         final double[] bias2 = new double[3];
17346         calibrator.getInitialBias(bias2);
17347         assertArrayEquals(bias1, bias2, 0.0);
17348         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17349         assertEquals(b1, ba);
17350         final Matrix b2 = new Matrix(3, 1);
17351         calibrator.getInitialBiasAsMatrix(b2);
17352         assertEquals(b1, b2);
17353         final Matrix ma1 = calibrator.getInitialMa();
17354         assertEquals(ma1, new Matrix(3, 3));
17355         final Matrix ma2 = new Matrix(3, 3);
17356         calibrator.getInitialMa(ma2);
17357         assertEquals(ma1, ma2);
17358         assertNull(calibrator.getMeasurements());
17359         assertTrue(calibrator.isCommonAxisUsed());
17360         assertSame(calibrator.getListener(), this);
17361         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17362         assertFalse(calibrator.isReady());
17363         assertFalse(calibrator.isRunning());
17364         assertNull(calibrator.getEstimatedBiases());
17365         assertFalse(calibrator.getEstimatedBiases(null));
17366         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17367         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17368         assertNull(calibrator.getEstimatedBiasFx());
17369         assertNull(calibrator.getEstimatedBiasFy());
17370         assertNull(calibrator.getEstimatedBiasFz());
17371         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17372         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17373         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17374         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17375         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17376         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17377         assertNull(calibrator.getEstimatedMa());
17378         assertNull(calibrator.getEstimatedSx());
17379         assertNull(calibrator.getEstimatedSy());
17380         assertNull(calibrator.getEstimatedSz());
17381         assertNull(calibrator.getEstimatedMxy());
17382         assertNull(calibrator.getEstimatedMxz());
17383         assertNull(calibrator.getEstimatedMyx());
17384         assertNull(calibrator.getEstimatedMyz());
17385         assertNull(calibrator.getEstimatedMzx());
17386         assertNull(calibrator.getEstimatedMzy());
17387         assertNull(calibrator.getEstimatedCovariance());
17388         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17389         assertNotNull(calibrator.getGroundTruthGravityNorm());
17390         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17391         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17392         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17393                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17394         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17395         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17396         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17397 
17398         // Force IllegalArgumentException
17399         calibrator = null;
17400         try {
17401             calibrator = new KnownGravityNormAccelerometerCalibrator(
17402                     -gravityNorm, true, bias, this);
17403             fail("IllegalArgumentException expected but not thrown");
17404         } catch (final IllegalArgumentException ignore) {
17405         }
17406         try {
17407             calibrator = new KnownGravityNormAccelerometerCalibrator(
17408                     gravityNorm, true, new double[1],
17409                     this);
17410             fail("IllegalArgumentException expected but not thrown");
17411         } catch (final IllegalArgumentException ignore) {
17412         }
17413         assertNull(calibrator);
17414     }
17415 
17416     @Test
17417     public void testConstructor139() throws WrongSizeException {
17418         final Collection<StandardDeviationBodyKinematics> measurements =
17419                 Collections.emptyList();
17420 
17421         final Matrix ba = generateBa();
17422         final double[] bias = ba.getBuffer();
17423         final double biasX = ba.getElementAtIndex(0);
17424         final double biasY = ba.getElementAtIndex(1);
17425         final double biasZ = ba.getElementAtIndex(2);
17426 
17427         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17428         final double latitude = Math.toRadians(
17429                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17430         final double longitude = Math.toRadians(
17431                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17432         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17433         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17434         final NEDVelocity nedVelocity = new NEDVelocity();
17435         final ECEFPosition ecefPosition = new ECEFPosition();
17436         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17437         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17438                 ecefPosition, ecefVelocity);
17439         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17440                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17441         final double gravityNorm = gravity.getNorm();
17442 
17443         KnownGravityNormAccelerometerCalibrator calibrator =
17444                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
17445                         measurements, true, bias);
17446 
17447         // check default values
17448         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17449         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17450         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17451         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17452         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17453         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17454         final Acceleration bx2 = new Acceleration(0.0,
17455                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17456         calibrator.getInitialBiasXAsAcceleration(bx2);
17457         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17458         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17459         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17460         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17461         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17462         final Acceleration by2 = new Acceleration(0.0,
17463                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17464         calibrator.getInitialBiasYAsAcceleration(by2);
17465         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17466         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17467         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17468         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17469         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17470         final Acceleration bz2 = new Acceleration(0.0,
17471                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17472         calibrator.getInitialBiasZAsAcceleration(bz2);
17473         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17474         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17475         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17476         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17477         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17478         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17479         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17480         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17481         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17482         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17483         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17484         final double[] bias1 = calibrator.getInitialBias();
17485         assertArrayEquals(bias1, bias, 0.0);
17486         final double[] bias2 = new double[3];
17487         calibrator.getInitialBias(bias2);
17488         assertArrayEquals(bias1, bias2, 0.0);
17489         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17490         assertEquals(b1, ba);
17491         final Matrix b2 = new Matrix(3, 1);
17492         calibrator.getInitialBiasAsMatrix(b2);
17493         assertEquals(b1, b2);
17494         final Matrix ma1 = calibrator.getInitialMa();
17495         assertEquals(ma1, new Matrix(3, 3));
17496         final Matrix ma2 = new Matrix(3, 3);
17497         calibrator.getInitialMa(ma2);
17498         assertEquals(ma1, ma2);
17499         assertSame(calibrator.getMeasurements(), measurements);
17500         assertTrue(calibrator.isCommonAxisUsed());
17501         assertNull(calibrator.getListener());
17502         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17503         assertFalse(calibrator.isReady());
17504         assertFalse(calibrator.isRunning());
17505         assertNull(calibrator.getEstimatedBiases());
17506         assertFalse(calibrator.getEstimatedBiases(null));
17507         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17508         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17509         assertNull(calibrator.getEstimatedBiasFx());
17510         assertNull(calibrator.getEstimatedBiasFy());
17511         assertNull(calibrator.getEstimatedBiasFz());
17512         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17513         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17514         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17515         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17516         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17517         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17518         assertNull(calibrator.getEstimatedMa());
17519         assertNull(calibrator.getEstimatedSx());
17520         assertNull(calibrator.getEstimatedSy());
17521         assertNull(calibrator.getEstimatedSz());
17522         assertNull(calibrator.getEstimatedMxy());
17523         assertNull(calibrator.getEstimatedMxz());
17524         assertNull(calibrator.getEstimatedMyx());
17525         assertNull(calibrator.getEstimatedMyz());
17526         assertNull(calibrator.getEstimatedMzx());
17527         assertNull(calibrator.getEstimatedMzy());
17528         assertNull(calibrator.getEstimatedCovariance());
17529         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17530         assertNotNull(calibrator.getGroundTruthGravityNorm());
17531         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17532         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17533         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17534                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17535         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17536         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17537         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17538 
17539         // Force IllegalArgumentException
17540         calibrator = null;
17541         try {
17542             calibrator = new KnownGravityNormAccelerometerCalibrator(
17543                     -gravityNorm, measurements, true, bias);
17544             fail("IllegalArgumentException expected but not thrown");
17545         } catch (final IllegalArgumentException ignore) {
17546         }
17547         try {
17548             calibrator = new KnownGravityNormAccelerometerCalibrator(
17549                     gravityNorm, measurements, true,
17550                     new double[1]);
17551             fail("IllegalArgumentException expected but not thrown");
17552         } catch (final IllegalArgumentException ignore) {
17553         }
17554         assertNull(calibrator);
17555     }
17556 
17557     @Test
17558     public void testConstructor140() throws WrongSizeException {
17559         final Collection<StandardDeviationBodyKinematics> measurements =
17560                 Collections.emptyList();
17561 
17562         final Matrix ba = generateBa();
17563         final double[] bias = ba.getBuffer();
17564         final double biasX = ba.getElementAtIndex(0);
17565         final double biasY = ba.getElementAtIndex(1);
17566         final double biasZ = ba.getElementAtIndex(2);
17567 
17568         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17569         final double latitude = Math.toRadians(
17570                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17571         final double longitude = Math.toRadians(
17572                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17573         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17574         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17575         final NEDVelocity nedVelocity = new NEDVelocity();
17576         final ECEFPosition ecefPosition = new ECEFPosition();
17577         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17578         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17579                 ecefPosition, ecefVelocity);
17580         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17581                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17582         final double gravityNorm = gravity.getNorm();
17583 
17584         KnownGravityNormAccelerometerCalibrator calibrator =
17585                 new KnownGravityNormAccelerometerCalibrator(
17586                         gravityNorm, measurements,
17587                         true, bias, this);
17588 
17589         // check default values
17590         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17591         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17592         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17593         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17594         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17595         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17596         final Acceleration bx2 = new Acceleration(0.0,
17597                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17598         calibrator.getInitialBiasXAsAcceleration(bx2);
17599         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17600         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17601         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17602         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17603         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17604         final Acceleration by2 = new Acceleration(0.0,
17605                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17606         calibrator.getInitialBiasYAsAcceleration(by2);
17607         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17608         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17609         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17610         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17611         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17612         final Acceleration bz2 = new Acceleration(0.0,
17613                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17614         calibrator.getInitialBiasZAsAcceleration(bz2);
17615         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17616         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17617         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17618         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17619         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17620         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17621         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17622         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17623         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17624         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17625         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17626         final double[] bias1 = calibrator.getInitialBias();
17627         assertArrayEquals(bias1, bias, 0.0);
17628         final double[] bias2 = new double[3];
17629         calibrator.getInitialBias(bias2);
17630         assertArrayEquals(bias1, bias2, 0.0);
17631         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17632         assertEquals(b1, ba);
17633         final Matrix b2 = new Matrix(3, 1);
17634         calibrator.getInitialBiasAsMatrix(b2);
17635         assertEquals(b1, b2);
17636         final Matrix ma1 = calibrator.getInitialMa();
17637         assertEquals(ma1, new Matrix(3, 3));
17638         final Matrix ma2 = new Matrix(3, 3);
17639         calibrator.getInitialMa(ma2);
17640         assertEquals(ma1, ma2);
17641         assertSame(calibrator.getMeasurements(), measurements);
17642         assertTrue(calibrator.isCommonAxisUsed());
17643         assertSame(calibrator.getListener(), this);
17644         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17645         assertFalse(calibrator.isReady());
17646         assertFalse(calibrator.isRunning());
17647         assertNull(calibrator.getEstimatedBiases());
17648         assertFalse(calibrator.getEstimatedBiases(null));
17649         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17650         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17651         assertNull(calibrator.getEstimatedBiasFx());
17652         assertNull(calibrator.getEstimatedBiasFy());
17653         assertNull(calibrator.getEstimatedBiasFz());
17654         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17655         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17656         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17657         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17658         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17659         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17660         assertNull(calibrator.getEstimatedMa());
17661         assertNull(calibrator.getEstimatedSx());
17662         assertNull(calibrator.getEstimatedSy());
17663         assertNull(calibrator.getEstimatedSz());
17664         assertNull(calibrator.getEstimatedMxy());
17665         assertNull(calibrator.getEstimatedMxz());
17666         assertNull(calibrator.getEstimatedMyx());
17667         assertNull(calibrator.getEstimatedMyz());
17668         assertNull(calibrator.getEstimatedMzx());
17669         assertNull(calibrator.getEstimatedMzy());
17670         assertNull(calibrator.getEstimatedCovariance());
17671         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17672         assertNotNull(calibrator.getGroundTruthGravityNorm());
17673         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17674         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17675         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17676                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17677         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17678         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17679         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17680 
17681         // Force IllegalArgumentException
17682         calibrator = null;
17683         try {
17684             calibrator = new KnownGravityNormAccelerometerCalibrator(
17685                     -gravityNorm, measurements,
17686                     true, bias, this);
17687             fail("IllegalArgumentException expected but not thrown");
17688         } catch (final IllegalArgumentException ignore) {
17689         }
17690         try {
17691             calibrator = new KnownGravityNormAccelerometerCalibrator(
17692                     gravityNorm, measurements, true,
17693                     new double[1], this);
17694             fail("IllegalArgumentException expected but not thrown");
17695         } catch (final IllegalArgumentException ignore) {
17696         }
17697         assertNull(calibrator);
17698     }
17699 
17700     @Test
17701     public void testConstructor141() throws WrongSizeException {
17702         final Matrix ba = generateBa();
17703         final double[] bias = ba.getBuffer();
17704         final double biasX = ba.getElementAtIndex(0);
17705         final double biasY = ba.getElementAtIndex(1);
17706         final double biasZ = ba.getElementAtIndex(2);
17707 
17708         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17709         final double latitude = Math.toRadians(
17710                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17711         final double longitude = Math.toRadians(
17712                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17713         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17714         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17715         final NEDVelocity nedVelocity = new NEDVelocity();
17716         final ECEFPosition ecefPosition = new ECEFPosition();
17717         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17718         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17719                 ecefPosition, ecefVelocity);
17720         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17721                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17722         final double gravityNorm = gravity.getNorm();
17723 
17724         KnownGravityNormAccelerometerCalibrator calibrator =
17725                 new KnownGravityNormAccelerometerCalibrator(
17726                         gravityNorm, ba);
17727 
17728         // check default values
17729         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17730         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17731         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17732         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17733         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17734         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17735         final Acceleration bx2 = new Acceleration(0.0,
17736                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17737         calibrator.getInitialBiasXAsAcceleration(bx2);
17738         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17739         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17740         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17741         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17742         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17743         final Acceleration by2 = new Acceleration(0.0,
17744                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17745         calibrator.getInitialBiasYAsAcceleration(by2);
17746         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17747         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17748         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17749         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17750         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17751         final Acceleration bz2 = new Acceleration(0.0,
17752                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17753         calibrator.getInitialBiasZAsAcceleration(bz2);
17754         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17755         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17756         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17757         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17758         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17759         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17760         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17761         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17762         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17763         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17764         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17765         final double[] bias1 = calibrator.getInitialBias();
17766         assertArrayEquals(bias1, bias, 0.0);
17767         final double[] bias2 = new double[3];
17768         calibrator.getInitialBias(bias2);
17769         assertArrayEquals(bias1, bias2, 0.0);
17770         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17771         assertEquals(b1, ba);
17772         final Matrix b2 = new Matrix(3, 1);
17773         calibrator.getInitialBiasAsMatrix(b2);
17774         assertEquals(b1, b2);
17775         final Matrix ma1 = calibrator.getInitialMa();
17776         assertEquals(ma1, new Matrix(3, 3));
17777         final Matrix ma2 = new Matrix(3, 3);
17778         calibrator.getInitialMa(ma2);
17779         assertEquals(ma1, ma2);
17780         assertNull(calibrator.getMeasurements());
17781         assertFalse(calibrator.isCommonAxisUsed());
17782         assertNull(calibrator.getListener());
17783         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17784         assertFalse(calibrator.isReady());
17785         assertFalse(calibrator.isRunning());
17786         assertNull(calibrator.getEstimatedBiases());
17787         assertFalse(calibrator.getEstimatedBiases(null));
17788         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17789         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17790         assertNull(calibrator.getEstimatedBiasFx());
17791         assertNull(calibrator.getEstimatedBiasFy());
17792         assertNull(calibrator.getEstimatedBiasFz());
17793         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17794         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17795         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17796         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17797         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17798         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17799         assertNull(calibrator.getEstimatedMa());
17800         assertNull(calibrator.getEstimatedSx());
17801         assertNull(calibrator.getEstimatedSy());
17802         assertNull(calibrator.getEstimatedSz());
17803         assertNull(calibrator.getEstimatedMxy());
17804         assertNull(calibrator.getEstimatedMxz());
17805         assertNull(calibrator.getEstimatedMyx());
17806         assertNull(calibrator.getEstimatedMyz());
17807         assertNull(calibrator.getEstimatedMzx());
17808         assertNull(calibrator.getEstimatedMzy());
17809         assertNull(calibrator.getEstimatedCovariance());
17810         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17811         assertNotNull(calibrator.getGroundTruthGravityNorm());
17812         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17813         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17814         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17815                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17816         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17817         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17818         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17819 
17820         // Force IllegalArgumentException
17821         calibrator = null;
17822         try {
17823             calibrator = new KnownGravityNormAccelerometerCalibrator(
17824                     -gravityNorm, ba);
17825             fail("IllegalArgumentException expected but not thrown");
17826         } catch (final IllegalArgumentException ignore) {
17827         }
17828         try {
17829             calibrator = new KnownGravityNormAccelerometerCalibrator(
17830                     gravityNorm, new Matrix(1, 1));
17831             fail("IllegalArgumentException expected but not thrown");
17832         } catch (final IllegalArgumentException ignore) {
17833         }
17834         try {
17835             calibrator = new KnownGravityNormAccelerometerCalibrator(
17836                     gravityNorm, new Matrix(1, 3));
17837             fail("IllegalArgumentException expected but not thrown");
17838         } catch (final IllegalArgumentException ignore) {
17839         }
17840         assertNull(calibrator);
17841     }
17842 
17843     @Test
17844     public void testConstructor142() throws WrongSizeException {
17845         final Matrix ba = generateBa();
17846         final double[] bias = ba.getBuffer();
17847         final double biasX = ba.getElementAtIndex(0);
17848         final double biasY = ba.getElementAtIndex(1);
17849         final double biasZ = ba.getElementAtIndex(2);
17850 
17851         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17852         final double latitude = Math.toRadians(
17853                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17854         final double longitude = Math.toRadians(
17855                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17856         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17857         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17858         final NEDVelocity nedVelocity = new NEDVelocity();
17859         final ECEFPosition ecefPosition = new ECEFPosition();
17860         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17861         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17862                 ecefPosition, ecefVelocity);
17863         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17864                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17865         final double gravityNorm = gravity.getNorm();
17866 
17867         KnownGravityNormAccelerometerCalibrator calibrator =
17868                 new KnownGravityNormAccelerometerCalibrator(
17869                         gravityNorm, ba, this);
17870 
17871         // check default values
17872         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17873         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17874         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17875         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17876         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17877         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17878         final Acceleration bx2 = new Acceleration(0.0,
17879                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17880         calibrator.getInitialBiasXAsAcceleration(bx2);
17881         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17882         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17883         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17884         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17885         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17886         final Acceleration by2 = new Acceleration(0.0,
17887                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17888         calibrator.getInitialBiasYAsAcceleration(by2);
17889         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17890         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17891         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17892         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17893         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17894         final Acceleration bz2 = new Acceleration(0.0,
17895                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17896         calibrator.getInitialBiasZAsAcceleration(bz2);
17897         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17898         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17899         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17900         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17901         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17902         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17903         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17904         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17905         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17906         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17907         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17908         final double[] bias1 = calibrator.getInitialBias();
17909         assertArrayEquals(bias1, bias, 0.0);
17910         final double[] bias2 = new double[3];
17911         calibrator.getInitialBias(bias2);
17912         assertArrayEquals(bias1, bias2, 0.0);
17913         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17914         assertEquals(b1, ba);
17915         final Matrix b2 = new Matrix(3, 1);
17916         calibrator.getInitialBiasAsMatrix(b2);
17917         assertEquals(b1, b2);
17918         final Matrix ma1 = calibrator.getInitialMa();
17919         assertEquals(ma1, new Matrix(3, 3));
17920         final Matrix ma2 = new Matrix(3, 3);
17921         calibrator.getInitialMa(ma2);
17922         assertEquals(ma1, ma2);
17923         assertNull(calibrator.getMeasurements());
17924         assertFalse(calibrator.isCommonAxisUsed());
17925         assertSame(calibrator.getListener(), this);
17926         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17927         assertFalse(calibrator.isReady());
17928         assertFalse(calibrator.isRunning());
17929         assertNull(calibrator.getEstimatedBiases());
17930         assertFalse(calibrator.getEstimatedBiases(null));
17931         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17932         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17933         assertNull(calibrator.getEstimatedBiasFx());
17934         assertNull(calibrator.getEstimatedBiasFy());
17935         assertNull(calibrator.getEstimatedBiasFz());
17936         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17937         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17938         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17939         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17940         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17941         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17942         assertNull(calibrator.getEstimatedMa());
17943         assertNull(calibrator.getEstimatedSx());
17944         assertNull(calibrator.getEstimatedSy());
17945         assertNull(calibrator.getEstimatedSz());
17946         assertNull(calibrator.getEstimatedMxy());
17947         assertNull(calibrator.getEstimatedMxz());
17948         assertNull(calibrator.getEstimatedMyx());
17949         assertNull(calibrator.getEstimatedMyz());
17950         assertNull(calibrator.getEstimatedMzx());
17951         assertNull(calibrator.getEstimatedMzy());
17952         assertNull(calibrator.getEstimatedCovariance());
17953         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17954         assertNotNull(calibrator.getGroundTruthGravityNorm());
17955         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17956         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17957         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17958                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17959         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17960         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17961         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17962 
17963         // Force IllegalArgumentException
17964         calibrator = null;
17965         try {
17966             calibrator = new KnownGravityNormAccelerometerCalibrator(
17967                     -gravityNorm, ba, this);
17968             fail("IllegalArgumentException expected but not thrown");
17969         } catch (final IllegalArgumentException ignore) {
17970         }
17971         try {
17972             calibrator = new KnownGravityNormAccelerometerCalibrator(
17973                     gravityNorm, new Matrix(1, 1), this);
17974             fail("IllegalArgumentException expected but not thrown");
17975         } catch (final IllegalArgumentException ignore) {
17976         }
17977         try {
17978             calibrator = new KnownGravityNormAccelerometerCalibrator(
17979                     gravityNorm, new Matrix(1, 3),
17980                     this);
17981             fail("IllegalArgumentException expected but not thrown");
17982         } catch (final IllegalArgumentException ignore) {
17983         }
17984         assertNull(calibrator);
17985     }
17986 
17987     @Test
17988     public void testConstructor143() throws WrongSizeException {
17989         final Collection<StandardDeviationBodyKinematics> measurements =
17990                 Collections.emptyList();
17991 
17992         final Matrix ba = generateBa();
17993         final double[] bias = ba.getBuffer();
17994         final double biasX = ba.getElementAtIndex(0);
17995         final double biasY = ba.getElementAtIndex(1);
17996         final double biasZ = ba.getElementAtIndex(2);
17997 
17998         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17999         final double latitude = Math.toRadians(
18000                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18001         final double longitude = Math.toRadians(
18002                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18003         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18004         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18005         final NEDVelocity nedVelocity = new NEDVelocity();
18006         final ECEFPosition ecefPosition = new ECEFPosition();
18007         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18008         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18009                 ecefPosition, ecefVelocity);
18010         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18011                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18012         final double gravityNorm = gravity.getNorm();
18013 
18014         KnownGravityNormAccelerometerCalibrator calibrator =
18015                 new KnownGravityNormAccelerometerCalibrator(
18016                         gravityNorm, measurements, ba);
18017 
18018         // check default values
18019         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18020         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18021         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18022         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18023         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18024         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18025         final Acceleration bx2 = new Acceleration(0.0,
18026                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18027         calibrator.getInitialBiasXAsAcceleration(bx2);
18028         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18029         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18030         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18031         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18032         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18033         final Acceleration by2 = new Acceleration(0.0,
18034                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18035         calibrator.getInitialBiasYAsAcceleration(by2);
18036         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18037         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18038         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18039         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18040         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18041         final Acceleration bz2 = new Acceleration(0.0,
18042                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18043         calibrator.getInitialBiasZAsAcceleration(bz2);
18044         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18045         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18046         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18047         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18048         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18049         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18050         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18051         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18052         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18053         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18054         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18055         final double[] bias1 = calibrator.getInitialBias();
18056         assertArrayEquals(bias1, bias, 0.0);
18057         final double[] bias2 = new double[3];
18058         calibrator.getInitialBias(bias2);
18059         assertArrayEquals(bias1, bias2, 0.0);
18060         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18061         assertEquals(b1, ba);
18062         final Matrix b2 = new Matrix(3, 1);
18063         calibrator.getInitialBiasAsMatrix(b2);
18064         assertEquals(b1, b2);
18065         final Matrix ma1 = calibrator.getInitialMa();
18066         assertEquals(ma1, new Matrix(3, 3));
18067         final Matrix ma2 = new Matrix(3, 3);
18068         calibrator.getInitialMa(ma2);
18069         assertEquals(ma1, ma2);
18070         assertSame(calibrator.getMeasurements(), measurements);
18071         assertFalse(calibrator.isCommonAxisUsed());
18072         assertNull(calibrator.getListener());
18073         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
18074         assertFalse(calibrator.isReady());
18075         assertFalse(calibrator.isRunning());
18076         assertNull(calibrator.getEstimatedBiases());
18077         assertFalse(calibrator.getEstimatedBiases(null));
18078         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18079         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18080         assertNull(calibrator.getEstimatedBiasFx());
18081         assertNull(calibrator.getEstimatedBiasFy());
18082         assertNull(calibrator.getEstimatedBiasFz());
18083         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18084         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18085         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18086         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18087         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18088         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18089         assertNull(calibrator.getEstimatedMa());
18090         assertNull(calibrator.getEstimatedSx());
18091         assertNull(calibrator.getEstimatedSy());
18092         assertNull(calibrator.getEstimatedSz());
18093         assertNull(calibrator.getEstimatedMxy());
18094         assertNull(calibrator.getEstimatedMxz());
18095         assertNull(calibrator.getEstimatedMyx());
18096         assertNull(calibrator.getEstimatedMyz());
18097         assertNull(calibrator.getEstimatedMzx());
18098         assertNull(calibrator.getEstimatedMzy());
18099         assertNull(calibrator.getEstimatedCovariance());
18100         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18101         assertNotNull(calibrator.getGroundTruthGravityNorm());
18102         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18103         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18104         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18105                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18106         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18107         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18108         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18109 
18110         // Force IllegalArgumentException
18111         calibrator = null;
18112         try {
18113             calibrator = new KnownGravityNormAccelerometerCalibrator(
18114                     -gravityNorm, measurements, ba);
18115             fail("IllegalArgumentException expected but not thrown");
18116         } catch (final IllegalArgumentException ignore) {
18117         }
18118         try {
18119             calibrator = new KnownGravityNormAccelerometerCalibrator(
18120                     gravityNorm, measurements,
18121                     new Matrix(1, 1));
18122             fail("IllegalArgumentException expected but not thrown");
18123         } catch (final IllegalArgumentException ignore) {
18124         }
18125         try {
18126             calibrator = new KnownGravityNormAccelerometerCalibrator(
18127                     gravityNorm, measurements,
18128                     new Matrix(1, 3));
18129             fail("IllegalArgumentException expected but not thrown");
18130         } catch (final IllegalArgumentException ignore) {
18131         }
18132         assertNull(calibrator);
18133     }
18134 
18135     @Test
18136     public void testConstructor144() throws WrongSizeException {
18137         final Collection<StandardDeviationBodyKinematics> measurements =
18138                 Collections.emptyList();
18139 
18140         final Matrix ba = generateBa();
18141         final double[] bias = ba.getBuffer();
18142         final double biasX = ba.getElementAtIndex(0);
18143         final double biasY = ba.getElementAtIndex(1);
18144         final double biasZ = ba.getElementAtIndex(2);
18145 
18146         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18147         final double latitude = Math.toRadians(
18148                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18149         final double longitude = Math.toRadians(
18150                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18151         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18152         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18153         final NEDVelocity nedVelocity = new NEDVelocity();
18154         final ECEFPosition ecefPosition = new ECEFPosition();
18155         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18156         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18157                 ecefPosition, ecefVelocity);
18158         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18159                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18160         final double gravityNorm = gravity.getNorm();
18161 
18162         KnownGravityNormAccelerometerCalibrator calibrator =
18163                 new KnownGravityNormAccelerometerCalibrator(
18164                         gravityNorm, measurements, ba, this);
18165 
18166         // check default values
18167         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18168         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18169         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18170         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18171         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18172         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18173         final Acceleration bx2 = new Acceleration(0.0,
18174                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18175         calibrator.getInitialBiasXAsAcceleration(bx2);
18176         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18177         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18178         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18179         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18180         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18181         final Acceleration by2 = new Acceleration(0.0,
18182                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18183         calibrator.getInitialBiasYAsAcceleration(by2);
18184         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18185         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18186         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18187         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18188         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18189         final Acceleration bz2 = new Acceleration(0.0,
18190                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18191         calibrator.getInitialBiasZAsAcceleration(bz2);
18192         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18193         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18194         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18195         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18196         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18197         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18198         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18199         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18200         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18201         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18202         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18203         final double[] bias1 = calibrator.getInitialBias();
18204         assertArrayEquals(bias1, bias, 0.0);
18205         final double[] bias2 = new double[3];
18206         calibrator.getInitialBias(bias2);
18207         assertArrayEquals(bias1, bias2, 0.0);
18208         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18209         assertEquals(b1, ba);
18210         final Matrix b2 = new Matrix(3, 1);
18211         calibrator.getInitialBiasAsMatrix(b2);
18212         assertEquals(b1, b2);
18213         final Matrix ma1 = calibrator.getInitialMa();
18214         assertEquals(ma1, new Matrix(3, 3));
18215         final Matrix ma2 = new Matrix(3, 3);
18216         calibrator.getInitialMa(ma2);
18217         assertEquals(ma1, ma2);
18218         assertSame(calibrator.getMeasurements(), measurements);
18219         assertFalse(calibrator.isCommonAxisUsed());
18220         assertSame(calibrator.getListener(), this);
18221         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
18222         assertFalse(calibrator.isReady());
18223         assertFalse(calibrator.isRunning());
18224         assertNull(calibrator.getEstimatedBiases());
18225         assertFalse(calibrator.getEstimatedBiases(null));
18226         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18227         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18228         assertNull(calibrator.getEstimatedBiasFx());
18229         assertNull(calibrator.getEstimatedBiasFy());
18230         assertNull(calibrator.getEstimatedBiasFz());
18231         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18232         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18233         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18234         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18235         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18236         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18237         assertNull(calibrator.getEstimatedMa());
18238         assertNull(calibrator.getEstimatedSx());
18239         assertNull(calibrator.getEstimatedSy());
18240         assertNull(calibrator.getEstimatedSz());
18241         assertNull(calibrator.getEstimatedMxy());
18242         assertNull(calibrator.getEstimatedMxz());
18243         assertNull(calibrator.getEstimatedMyx());
18244         assertNull(calibrator.getEstimatedMyz());
18245         assertNull(calibrator.getEstimatedMzx());
18246         assertNull(calibrator.getEstimatedMzy());
18247         assertNull(calibrator.getEstimatedCovariance());
18248         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18249         assertNotNull(calibrator.getGroundTruthGravityNorm());
18250         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18251         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18252         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18253                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18254         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18255         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18256         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18257 
18258         // Force IllegalArgumentException
18259         calibrator = null;
18260         try {
18261             calibrator = new KnownGravityNormAccelerometerCalibrator(
18262                     -gravityNorm, measurements, ba, this);
18263             fail("IllegalArgumentException expected but not thrown");
18264         } catch (final IllegalArgumentException ignore) {
18265         }
18266         try {
18267             calibrator = new KnownGravityNormAccelerometerCalibrator(
18268                     gravityNorm, measurements,
18269                     new Matrix(1, 1), this);
18270             fail("IllegalArgumentException expected but not thrown");
18271         } catch (final IllegalArgumentException ignore) {
18272         }
18273         try {
18274             calibrator = new KnownGravityNormAccelerometerCalibrator(
18275                     gravityNorm, measurements,
18276                     new Matrix(1, 3), this);
18277             fail("IllegalArgumentException expected but not thrown");
18278         } catch (final IllegalArgumentException ignore) {
18279         }
18280         assertNull(calibrator);
18281     }
18282 
18283     @Test
18284     public void testConstructor145() throws WrongSizeException {
18285         final Matrix ba = generateBa();
18286         final double[] bias = ba.getBuffer();
18287         final double biasX = ba.getElementAtIndex(0);
18288         final double biasY = ba.getElementAtIndex(1);
18289         final double biasZ = ba.getElementAtIndex(2);
18290 
18291         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18292         final double latitude = Math.toRadians(
18293                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18294         final double longitude = Math.toRadians(
18295                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18296         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18297         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18298         final NEDVelocity nedVelocity = new NEDVelocity();
18299         final ECEFPosition ecefPosition = new ECEFPosition();
18300         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18301         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18302                 ecefPosition, ecefVelocity);
18303         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18304                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18305         final double gravityNorm = gravity.getNorm();
18306 
18307         KnownGravityNormAccelerometerCalibrator calibrator =
18308                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
18309                         true, ba);
18310 
18311         // check default values
18312         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18313         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18314         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18315         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18316         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18317         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18318         final Acceleration bx2 = new Acceleration(0.0,
18319                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18320         calibrator.getInitialBiasXAsAcceleration(bx2);
18321         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18322         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18323         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18324         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18325         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18326         final Acceleration by2 = new Acceleration(0.0,
18327                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18328         calibrator.getInitialBiasYAsAcceleration(by2);
18329         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18330         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18331         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18332         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18333         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18334         final Acceleration bz2 = new Acceleration(0.0,
18335                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18336         calibrator.getInitialBiasZAsAcceleration(bz2);
18337         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18338         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18339         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18340         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18341         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18342         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18343         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18344         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18345         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18346         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18347         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18348         final double[] bias1 = calibrator.getInitialBias();
18349         assertArrayEquals(bias1, bias, 0.0);
18350         final double[] bias2 = new double[3];
18351         calibrator.getInitialBias(bias2);
18352         assertArrayEquals(bias1, bias2, 0.0);
18353         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18354         assertEquals(b1, ba);
18355         final Matrix b2 = new Matrix(3, 1);
18356         calibrator.getInitialBiasAsMatrix(b2);
18357         assertEquals(b1, b2);
18358         final Matrix ma1 = calibrator.getInitialMa();
18359         assertEquals(ma1, new Matrix(3, 3));
18360         final Matrix ma2 = new Matrix(3, 3);
18361         calibrator.getInitialMa(ma2);
18362         assertEquals(ma1, ma2);
18363         assertNull(calibrator.getMeasurements());
18364         assertTrue(calibrator.isCommonAxisUsed());
18365         assertNull(calibrator.getListener());
18366         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18367         assertFalse(calibrator.isReady());
18368         assertFalse(calibrator.isRunning());
18369         assertNull(calibrator.getEstimatedBiases());
18370         assertFalse(calibrator.getEstimatedBiases(null));
18371         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18372         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18373         assertNull(calibrator.getEstimatedBiasFx());
18374         assertNull(calibrator.getEstimatedBiasFy());
18375         assertNull(calibrator.getEstimatedBiasFz());
18376         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18377         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18378         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18379         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18380         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18381         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18382         assertNull(calibrator.getEstimatedMa());
18383         assertNull(calibrator.getEstimatedSx());
18384         assertNull(calibrator.getEstimatedSy());
18385         assertNull(calibrator.getEstimatedSz());
18386         assertNull(calibrator.getEstimatedMxy());
18387         assertNull(calibrator.getEstimatedMxz());
18388         assertNull(calibrator.getEstimatedMyx());
18389         assertNull(calibrator.getEstimatedMyz());
18390         assertNull(calibrator.getEstimatedMzx());
18391         assertNull(calibrator.getEstimatedMzy());
18392         assertNull(calibrator.getEstimatedCovariance());
18393         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18394         assertNotNull(calibrator.getGroundTruthGravityNorm());
18395         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18396         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18397         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18398                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18399         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18400         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18401         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18402 
18403         // Force IllegalArgumentException
18404         calibrator = null;
18405         try {
18406             calibrator = new KnownGravityNormAccelerometerCalibrator(
18407                     -gravityNorm, true, ba);
18408             fail("IllegalArgumentException expected but not thrown");
18409         } catch (final IllegalArgumentException ignore) {
18410         }
18411         try {
18412             calibrator = new KnownGravityNormAccelerometerCalibrator(
18413                     gravityNorm, true,
18414                     new Matrix(1, 1));
18415             fail("IllegalArgumentException expected but not thrown");
18416         } catch (final IllegalArgumentException ignore) {
18417         }
18418         try {
18419             calibrator = new KnownGravityNormAccelerometerCalibrator(
18420                     gravityNorm, true,
18421                     new Matrix(1, 3));
18422             fail("IllegalArgumentException expected but not thrown");
18423         } catch (final IllegalArgumentException ignore) {
18424         }
18425         assertNull(calibrator);
18426     }
18427 
18428     @Test
18429     public void testConstructor146() throws WrongSizeException {
18430         final Matrix ba = generateBa();
18431         final double[] bias = ba.getBuffer();
18432         final double biasX = ba.getElementAtIndex(0);
18433         final double biasY = ba.getElementAtIndex(1);
18434         final double biasZ = ba.getElementAtIndex(2);
18435 
18436         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18437         final double latitude = Math.toRadians(
18438                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18439         final double longitude = Math.toRadians(
18440                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18441         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18442         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18443         final NEDVelocity nedVelocity = new NEDVelocity();
18444         final ECEFPosition ecefPosition = new ECEFPosition();
18445         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18446         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18447                 ecefPosition, ecefVelocity);
18448         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18449                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18450         final double gravityNorm = gravity.getNorm();
18451 
18452         KnownGravityNormAccelerometerCalibrator calibrator =
18453                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
18454                         true, ba, this);
18455 
18456         // check default values
18457         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18458         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18459         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18460         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18461         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18462         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18463         final Acceleration bx2 = new Acceleration(0.0,
18464                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18465         calibrator.getInitialBiasXAsAcceleration(bx2);
18466         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18467         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18468         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18469         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18470         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18471         final Acceleration by2 = new Acceleration(0.0,
18472                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18473         calibrator.getInitialBiasYAsAcceleration(by2);
18474         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18475         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18476         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18477         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18478         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18479         final Acceleration bz2 = new Acceleration(0.0,
18480                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18481         calibrator.getInitialBiasZAsAcceleration(bz2);
18482         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18483         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18484         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18485         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18486         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18487         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18488         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18489         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18490         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18491         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18492         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18493         final double[] bias1 = calibrator.getInitialBias();
18494         assertArrayEquals(bias1, bias, 0.0);
18495         final double[] bias2 = new double[3];
18496         calibrator.getInitialBias(bias2);
18497         assertArrayEquals(bias1, bias2, 0.0);
18498         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18499         assertEquals(b1, ba);
18500         final Matrix b2 = new Matrix(3, 1);
18501         calibrator.getInitialBiasAsMatrix(b2);
18502         assertEquals(b1, b2);
18503         final Matrix ma1 = calibrator.getInitialMa();
18504         assertEquals(ma1, new Matrix(3, 3));
18505         final Matrix ma2 = new Matrix(3, 3);
18506         calibrator.getInitialMa(ma2);
18507         assertEquals(ma1, ma2);
18508         assertNull(calibrator.getMeasurements());
18509         assertTrue(calibrator.isCommonAxisUsed());
18510         assertSame(calibrator.getListener(), this);
18511         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18512         assertFalse(calibrator.isReady());
18513         assertFalse(calibrator.isRunning());
18514         assertNull(calibrator.getEstimatedBiases());
18515         assertFalse(calibrator.getEstimatedBiases(null));
18516         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18517         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18518         assertNull(calibrator.getEstimatedBiasFx());
18519         assertNull(calibrator.getEstimatedBiasFy());
18520         assertNull(calibrator.getEstimatedBiasFz());
18521         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18522         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18523         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18524         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18525         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18526         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18527         assertNull(calibrator.getEstimatedMa());
18528         assertNull(calibrator.getEstimatedSx());
18529         assertNull(calibrator.getEstimatedSy());
18530         assertNull(calibrator.getEstimatedSz());
18531         assertNull(calibrator.getEstimatedMxy());
18532         assertNull(calibrator.getEstimatedMxz());
18533         assertNull(calibrator.getEstimatedMyx());
18534         assertNull(calibrator.getEstimatedMyz());
18535         assertNull(calibrator.getEstimatedMzx());
18536         assertNull(calibrator.getEstimatedMzy());
18537         assertNull(calibrator.getEstimatedCovariance());
18538         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18539         assertNotNull(calibrator.getGroundTruthGravityNorm());
18540         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18541         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18542         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18543                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18544         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18545         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18546         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18547 
18548         // Force IllegalArgumentException
18549         calibrator = null;
18550         try {
18551             calibrator = new KnownGravityNormAccelerometerCalibrator(
18552                     -gravityNorm, true, ba, this);
18553             fail("IllegalArgumentException expected but not thrown");
18554         } catch (final IllegalArgumentException ignore) {
18555         }
18556         try {
18557             calibrator = new KnownGravityNormAccelerometerCalibrator(
18558                     gravityNorm, true, new Matrix(1, 1),
18559                     this);
18560             fail("IllegalArgumentException expected but not thrown");
18561         } catch (final IllegalArgumentException ignore) {
18562         }
18563         try {
18564             calibrator = new KnownGravityNormAccelerometerCalibrator(
18565                     gravityNorm, true, new Matrix(1, 3),
18566                     this);
18567             fail("IllegalArgumentException expected but not thrown");
18568         } catch (final IllegalArgumentException ignore) {
18569         }
18570         assertNull(calibrator);
18571     }
18572 
18573     @Test
18574     public void testConstructor147() throws WrongSizeException {
18575         final Collection<StandardDeviationBodyKinematics> measurements =
18576                 Collections.emptyList();
18577 
18578         final Matrix ba = generateBa();
18579         final double[] bias = ba.getBuffer();
18580         final double biasX = ba.getElementAtIndex(0);
18581         final double biasY = ba.getElementAtIndex(1);
18582         final double biasZ = ba.getElementAtIndex(2);
18583 
18584         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18585         final double latitude = Math.toRadians(
18586                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18587         final double longitude = Math.toRadians(
18588                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18589         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18590         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18591         final NEDVelocity nedVelocity = new NEDVelocity();
18592         final ECEFPosition ecefPosition = new ECEFPosition();
18593         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18594         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18595                 ecefPosition, ecefVelocity);
18596         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18597                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18598         final double gravityNorm = gravity.getNorm();
18599 
18600         KnownGravityNormAccelerometerCalibrator calibrator =
18601                 new KnownGravityNormAccelerometerCalibrator(
18602                         gravityNorm, measurements,
18603                         true, ba);
18604 
18605         // check default values
18606         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18607         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18608         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18609         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18610         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18611         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18612         final Acceleration bx2 = new Acceleration(0.0,
18613                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18614         calibrator.getInitialBiasXAsAcceleration(bx2);
18615         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18616         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18617         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18618         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18619         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18620         final Acceleration by2 = new Acceleration(0.0,
18621                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18622         calibrator.getInitialBiasYAsAcceleration(by2);
18623         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18624         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18625         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18626         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18627         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18628         final Acceleration bz2 = new Acceleration(0.0,
18629                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18630         calibrator.getInitialBiasZAsAcceleration(bz2);
18631         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18632         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18633         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18634         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18635         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18636         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18637         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18638         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18639         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18640         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18641         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18642         final double[] bias1 = calibrator.getInitialBias();
18643         assertArrayEquals(bias1, bias, 0.0);
18644         final double[] bias2 = new double[3];
18645         calibrator.getInitialBias(bias2);
18646         assertArrayEquals(bias1, bias2, 0.0);
18647         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18648         assertEquals(b1, ba);
18649         final Matrix b2 = new Matrix(3, 1);
18650         calibrator.getInitialBiasAsMatrix(b2);
18651         assertEquals(b1, b2);
18652         final Matrix ma1 = calibrator.getInitialMa();
18653         assertEquals(ma1, new Matrix(3, 3));
18654         final Matrix ma2 = new Matrix(3, 3);
18655         calibrator.getInitialMa(ma2);
18656         assertEquals(ma1, ma2);
18657         assertSame(calibrator.getMeasurements(), measurements);
18658         assertTrue(calibrator.isCommonAxisUsed());
18659         assertNull(calibrator.getListener());
18660         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18661         assertFalse(calibrator.isReady());
18662         assertFalse(calibrator.isRunning());
18663         assertNull(calibrator.getEstimatedBiases());
18664         assertFalse(calibrator.getEstimatedBiases(null));
18665         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18666         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18667         assertNull(calibrator.getEstimatedBiasFx());
18668         assertNull(calibrator.getEstimatedBiasFy());
18669         assertNull(calibrator.getEstimatedBiasFz());
18670         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18671         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18672         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18673         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18674         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18675         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18676         assertNull(calibrator.getEstimatedMa());
18677         assertNull(calibrator.getEstimatedSx());
18678         assertNull(calibrator.getEstimatedSy());
18679         assertNull(calibrator.getEstimatedSz());
18680         assertNull(calibrator.getEstimatedMxy());
18681         assertNull(calibrator.getEstimatedMxz());
18682         assertNull(calibrator.getEstimatedMyx());
18683         assertNull(calibrator.getEstimatedMyz());
18684         assertNull(calibrator.getEstimatedMzx());
18685         assertNull(calibrator.getEstimatedMzy());
18686         assertNull(calibrator.getEstimatedCovariance());
18687         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18688         assertNotNull(calibrator.getGroundTruthGravityNorm());
18689         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18690         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18691         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18692                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18693         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18694         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18695         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18696 
18697         // Force IllegalArgumentException
18698         calibrator = null;
18699         try {
18700             calibrator = new KnownGravityNormAccelerometerCalibrator(
18701                     -gravityNorm, measurements,
18702                     true, ba);
18703             fail("IllegalArgumentException expected but not thrown");
18704         } catch (final IllegalArgumentException ignore) {
18705         }
18706         try {
18707             calibrator = new KnownGravityNormAccelerometerCalibrator(
18708                     gravityNorm, measurements, true,
18709                     new Matrix(1, 1));
18710             fail("IllegalArgumentException expected but not thrown");
18711         } catch (final IllegalArgumentException ignore) {
18712         }
18713         try {
18714             calibrator = new KnownGravityNormAccelerometerCalibrator(
18715                     gravityNorm, measurements, true,
18716                     new Matrix(1, 3));
18717             fail("IllegalArgumentException expected but not thrown");
18718         } catch (final IllegalArgumentException ignore) {
18719         }
18720         assertNull(calibrator);
18721     }
18722 
18723     @Test
18724     public void testConstructor148() throws WrongSizeException {
18725         final Collection<StandardDeviationBodyKinematics> measurements =
18726                 Collections.emptyList();
18727 
18728         final Matrix ba = generateBa();
18729         final double[] bias = ba.getBuffer();
18730         final double biasX = ba.getElementAtIndex(0);
18731         final double biasY = ba.getElementAtIndex(1);
18732         final double biasZ = ba.getElementAtIndex(2);
18733 
18734         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18735         final double latitude = Math.toRadians(
18736                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18737         final double longitude = Math.toRadians(
18738                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18739         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18740         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18741         final NEDVelocity nedVelocity = new NEDVelocity();
18742         final ECEFPosition ecefPosition = new ECEFPosition();
18743         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18744         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18745                 ecefPosition, ecefVelocity);
18746         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18747                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18748         final double gravityNorm = gravity.getNorm();
18749 
18750         KnownGravityNormAccelerometerCalibrator calibrator =
18751                 new KnownGravityNormAccelerometerCalibrator(
18752                         gravityNorm, measurements,
18753                         true, ba, this);
18754 
18755         // check default values
18756         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18757         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18758         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18759         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18760         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18761         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18762         final Acceleration bx2 = new Acceleration(0.0,
18763                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18764         calibrator.getInitialBiasXAsAcceleration(bx2);
18765         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18766         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18767         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18768         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18769         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18770         final Acceleration by2 = new Acceleration(0.0,
18771                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18772         calibrator.getInitialBiasYAsAcceleration(by2);
18773         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18774         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18775         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18776         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18777         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18778         final Acceleration bz2 = new Acceleration(0.0,
18779                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18780         calibrator.getInitialBiasZAsAcceleration(bz2);
18781         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18782         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18783         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18784         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18785         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18786         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18787         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18788         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18789         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18790         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18791         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18792         final double[] bias1 = calibrator.getInitialBias();
18793         assertArrayEquals(bias1, bias, 0.0);
18794         final double[] bias2 = new double[3];
18795         calibrator.getInitialBias(bias2);
18796         assertArrayEquals(bias1, bias2, 0.0);
18797         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18798         assertEquals(b1, ba);
18799         final Matrix b2 = new Matrix(3, 1);
18800         calibrator.getInitialBiasAsMatrix(b2);
18801         assertEquals(b1, b2);
18802         final Matrix ma1 = calibrator.getInitialMa();
18803         assertEquals(ma1, new Matrix(3, 3));
18804         final Matrix ma2 = new Matrix(3, 3);
18805         calibrator.getInitialMa(ma2);
18806         assertEquals(ma1, ma2);
18807         assertSame(calibrator.getMeasurements(), measurements);
18808         assertTrue(calibrator.isCommonAxisUsed());
18809         assertSame(calibrator.getListener(), this);
18810         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18811         assertFalse(calibrator.isReady());
18812         assertFalse(calibrator.isRunning());
18813         assertNull(calibrator.getEstimatedBiases());
18814         assertFalse(calibrator.getEstimatedBiases(null));
18815         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18816         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18817         assertNull(calibrator.getEstimatedBiasFx());
18818         assertNull(calibrator.getEstimatedBiasFy());
18819         assertNull(calibrator.getEstimatedBiasFz());
18820         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18821         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18822         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18823         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18824         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18825         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18826         assertNull(calibrator.getEstimatedMa());
18827         assertNull(calibrator.getEstimatedSx());
18828         assertNull(calibrator.getEstimatedSy());
18829         assertNull(calibrator.getEstimatedSz());
18830         assertNull(calibrator.getEstimatedMxy());
18831         assertNull(calibrator.getEstimatedMxz());
18832         assertNull(calibrator.getEstimatedMyx());
18833         assertNull(calibrator.getEstimatedMyz());
18834         assertNull(calibrator.getEstimatedMzx());
18835         assertNull(calibrator.getEstimatedMzy());
18836         assertNull(calibrator.getEstimatedCovariance());
18837         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18838         assertNotNull(calibrator.getGroundTruthGravityNorm());
18839         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18840         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18841         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18842                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18843         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18844         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18845         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18846 
18847         // Force IllegalArgumentException
18848         calibrator = null;
18849         try {
18850             calibrator = new KnownGravityNormAccelerometerCalibrator(
18851                     -gravityNorm, measurements,
18852                     true, ba, this);
18853             fail("IllegalArgumentException expected but not thrown");
18854         } catch (final IllegalArgumentException ignore) {
18855         }
18856         try {
18857             calibrator = new KnownGravityNormAccelerometerCalibrator(
18858                     gravityNorm, measurements, true,
18859                     new Matrix(1, 1), this);
18860             fail("IllegalArgumentException expected but not thrown");
18861         } catch (final IllegalArgumentException ignore) {
18862         }
18863         try {
18864             calibrator = new KnownGravityNormAccelerometerCalibrator(
18865                     gravityNorm, measurements, true,
18866                     new Matrix(1, 3), this);
18867             fail("IllegalArgumentException expected but not thrown");
18868         } catch (final IllegalArgumentException ignore) {
18869         }
18870         assertNull(calibrator);
18871     }
18872 
18873     @Test
18874     public void testConstructor149() throws WrongSizeException {
18875         final Matrix ba = generateBa();
18876         final double[] bias = ba.getBuffer();
18877         final double biasX = ba.getElementAtIndex(0);
18878         final double biasY = ba.getElementAtIndex(1);
18879         final double biasZ = ba.getElementAtIndex(2);
18880 
18881         final Matrix ma = generateMaCommonAxis();
18882         final double sx = ma.getElementAt(0, 0);
18883         final double sy = ma.getElementAt(1, 1);
18884         final double sz = ma.getElementAt(2, 2);
18885         final double mxy = ma.getElementAt(0, 1);
18886         final double mxz = ma.getElementAt(0, 2);
18887         final double myx = ma.getElementAt(1, 0);
18888         final double myz = ma.getElementAt(1, 2);
18889         final double mzx = ma.getElementAt(2, 0);
18890         final double mzy = ma.getElementAt(2, 1);
18891 
18892         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18893         final double latitude = Math.toRadians(
18894                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18895         final double longitude = Math.toRadians(
18896                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18897         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18898         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18899         final NEDVelocity nedVelocity = new NEDVelocity();
18900         final ECEFPosition ecefPosition = new ECEFPosition();
18901         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18902         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18903                 ecefPosition, ecefVelocity);
18904         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18905                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18906         final double gravityNorm = gravity.getNorm();
18907 
18908         KnownGravityNormAccelerometerCalibrator calibrator =
18909                 new KnownGravityNormAccelerometerCalibrator(
18910                         gravityNorm, ba, ma);
18911 
18912         // check default values
18913         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18914         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18915         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18916         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18917         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18918         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18919         final Acceleration bx2 = new Acceleration(0.0,
18920                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18921         calibrator.getInitialBiasXAsAcceleration(bx2);
18922         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18923         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18924         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18925         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18926         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18927         final Acceleration by2 = new Acceleration(0.0,
18928                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18929         calibrator.getInitialBiasYAsAcceleration(by2);
18930         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18931         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18932         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18933         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18934         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18935         final Acceleration bz2 = new Acceleration(0.0,
18936                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18937         calibrator.getInitialBiasZAsAcceleration(bz2);
18938         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18939         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18940         assertEquals(calibrator.getInitialSx(), sx, 0.0);
18941         assertEquals(calibrator.getInitialSy(), sy, 0.0);
18942         assertEquals(calibrator.getInitialSz(), sz, 0.0);
18943         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18944         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18945         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18946         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18947         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18948         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18949         final double[] bias1 = calibrator.getInitialBias();
18950         assertArrayEquals(bias1, bias, 0.0);
18951         final double[] bias2 = new double[3];
18952         calibrator.getInitialBias(bias2);
18953         assertArrayEquals(bias1, bias2, 0.0);
18954         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18955         assertEquals(b1, ba);
18956         final Matrix b2 = new Matrix(3, 1);
18957         calibrator.getInitialBiasAsMatrix(b2);
18958         assertEquals(b1, b2);
18959         final Matrix ma1 = new Matrix(3, 3);
18960         ma1.setSubmatrix(0, 0,
18961                 2, 2,
18962                 new double[]{sx, myx, mzx,
18963                         mxy, sy, mzy,
18964                         mxz, myz, sz});
18965         assertEquals(calibrator.getInitialMa(), ma1);
18966         final Matrix ma2 = new Matrix(3, 3);
18967         calibrator.getInitialMa(ma2);
18968         assertEquals(ma1, ma2);
18969         assertNull(calibrator.getMeasurements());
18970         assertFalse(calibrator.isCommonAxisUsed());
18971         assertNull(calibrator.getListener());
18972         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
18973         assertFalse(calibrator.isReady());
18974         assertFalse(calibrator.isRunning());
18975         assertNull(calibrator.getEstimatedBiases());
18976         assertFalse(calibrator.getEstimatedBiases(null));
18977         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18978         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18979         assertNull(calibrator.getEstimatedBiasFx());
18980         assertNull(calibrator.getEstimatedBiasFy());
18981         assertNull(calibrator.getEstimatedBiasFz());
18982         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18983         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18984         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18985         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18986         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18987         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18988         assertNull(calibrator.getEstimatedMa());
18989         assertNull(calibrator.getEstimatedSx());
18990         assertNull(calibrator.getEstimatedSy());
18991         assertNull(calibrator.getEstimatedSz());
18992         assertNull(calibrator.getEstimatedMxy());
18993         assertNull(calibrator.getEstimatedMxz());
18994         assertNull(calibrator.getEstimatedMyx());
18995         assertNull(calibrator.getEstimatedMyz());
18996         assertNull(calibrator.getEstimatedMzx());
18997         assertNull(calibrator.getEstimatedMzy());
18998         assertNull(calibrator.getEstimatedCovariance());
18999         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19000         assertNotNull(calibrator.getGroundTruthGravityNorm());
19001         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19002         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19003         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19004                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19005         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19006         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19007         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19008 
19009         // Force IllegalArgumentException
19010         calibrator = null;
19011         try {
19012             calibrator = new KnownGravityNormAccelerometerCalibrator(
19013                     -gravityNorm, ba, ma);
19014             fail("IllegalArgumentException expected but not thrown");
19015         } catch (final IllegalArgumentException ignore) {
19016         }
19017         try {
19018             calibrator = new KnownGravityNormAccelerometerCalibrator(
19019                     gravityNorm, new Matrix(1, 1), ma);
19020             fail("IllegalArgumentException expected but not thrown");
19021         } catch (final IllegalArgumentException ignore) {
19022         }
19023         try {
19024             calibrator = new KnownGravityNormAccelerometerCalibrator(
19025                     gravityNorm, new Matrix(1, 3), ma);
19026             fail("IllegalArgumentException expected but not thrown");
19027         } catch (final IllegalArgumentException ignore) {
19028         }
19029         try {
19030             calibrator = new KnownGravityNormAccelerometerCalibrator(
19031                     gravityNorm, ba, new Matrix(1, 3));
19032             fail("IllegalArgumentException expected but not thrown");
19033         } catch (final IllegalArgumentException ignore) {
19034         }
19035         try {
19036             calibrator = new KnownGravityNormAccelerometerCalibrator(
19037                     gravityNorm, ba, new Matrix(3, 1));
19038             fail("IllegalArgumentException expected but not thrown");
19039         } catch (final IllegalArgumentException ignore) {
19040         }
19041         assertNull(calibrator);
19042     }
19043 
19044     @Test
19045     public void testConstructor150() throws WrongSizeException {
19046         final Matrix ba = generateBa();
19047         final double[] bias = ba.getBuffer();
19048         final double biasX = ba.getElementAtIndex(0);
19049         final double biasY = ba.getElementAtIndex(1);
19050         final double biasZ = ba.getElementAtIndex(2);
19051 
19052         final Matrix ma = generateMaCommonAxis();
19053         final double sx = ma.getElementAt(0, 0);
19054         final double sy = ma.getElementAt(1, 1);
19055         final double sz = ma.getElementAt(2, 2);
19056         final double mxy = ma.getElementAt(0, 1);
19057         final double mxz = ma.getElementAt(0, 2);
19058         final double myx = ma.getElementAt(1, 0);
19059         final double myz = ma.getElementAt(1, 2);
19060         final double mzx = ma.getElementAt(2, 0);
19061         final double mzy = ma.getElementAt(2, 1);
19062 
19063         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19064         final double latitude = Math.toRadians(
19065                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19066         final double longitude = Math.toRadians(
19067                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19068         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19069         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19070         final NEDVelocity nedVelocity = new NEDVelocity();
19071         final ECEFPosition ecefPosition = new ECEFPosition();
19072         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19073         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19074                 ecefPosition, ecefVelocity);
19075         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19076                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19077         final double gravityNorm = gravity.getNorm();
19078 
19079         KnownGravityNormAccelerometerCalibrator calibrator =
19080                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
19081                         ba, ma, this);
19082 
19083         // check default values
19084         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19085         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19086         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19087         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19088         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19089         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19090         final Acceleration bx2 = new Acceleration(0.0,
19091                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19092         calibrator.getInitialBiasXAsAcceleration(bx2);
19093         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19094         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19095         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19096         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19097         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19098         final Acceleration by2 = new Acceleration(0.0,
19099                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19100         calibrator.getInitialBiasYAsAcceleration(by2);
19101         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19102         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19103         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19104         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19105         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19106         final Acceleration bz2 = new Acceleration(0.0,
19107                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19108         calibrator.getInitialBiasZAsAcceleration(bz2);
19109         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19110         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19111         assertEquals(calibrator.getInitialSx(), sx, 0.0);
19112         assertEquals(calibrator.getInitialSy(), sy, 0.0);
19113         assertEquals(calibrator.getInitialSz(), sz, 0.0);
19114         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19115         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19116         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19117         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19118         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19119         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19120         final double[] bias1 = calibrator.getInitialBias();
19121         assertArrayEquals(bias1, bias, 0.0);
19122         final double[] bias2 = new double[3];
19123         calibrator.getInitialBias(bias2);
19124         assertArrayEquals(bias1, bias2, 0.0);
19125         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19126         assertEquals(b1, ba);
19127         final Matrix b2 = new Matrix(3, 1);
19128         calibrator.getInitialBiasAsMatrix(b2);
19129         assertEquals(b1, b2);
19130         final Matrix ma1 = new Matrix(3, 3);
19131         ma1.setSubmatrix(0, 0,
19132                 2, 2,
19133                 new double[]{sx, myx, mzx,
19134                         mxy, sy, mzy,
19135                         mxz, myz, sz});
19136         assertEquals(calibrator.getInitialMa(), ma1);
19137         final Matrix ma2 = new Matrix(3, 3);
19138         calibrator.getInitialMa(ma2);
19139         assertEquals(ma1, ma2);
19140         assertNull(calibrator.getMeasurements());
19141         assertFalse(calibrator.isCommonAxisUsed());
19142         assertSame(calibrator.getListener(), this);
19143         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19144         assertFalse(calibrator.isReady());
19145         assertFalse(calibrator.isRunning());
19146         assertNull(calibrator.getEstimatedBiases());
19147         assertFalse(calibrator.getEstimatedBiases(null));
19148         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19149         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19150         assertNull(calibrator.getEstimatedBiasFx());
19151         assertNull(calibrator.getEstimatedBiasFy());
19152         assertNull(calibrator.getEstimatedBiasFz());
19153         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19154         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19155         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19156         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19157         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19158         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19159         assertNull(calibrator.getEstimatedMa());
19160         assertNull(calibrator.getEstimatedSx());
19161         assertNull(calibrator.getEstimatedSy());
19162         assertNull(calibrator.getEstimatedSz());
19163         assertNull(calibrator.getEstimatedMxy());
19164         assertNull(calibrator.getEstimatedMxz());
19165         assertNull(calibrator.getEstimatedMyx());
19166         assertNull(calibrator.getEstimatedMyz());
19167         assertNull(calibrator.getEstimatedMzx());
19168         assertNull(calibrator.getEstimatedMzy());
19169         assertNull(calibrator.getEstimatedCovariance());
19170         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19171         assertNotNull(calibrator.getGroundTruthGravityNorm());
19172         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19173         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19174         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19175                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19176         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19177         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19178         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19179 
19180         // Force IllegalArgumentException
19181         calibrator = null;
19182         try {
19183             calibrator = new KnownGravityNormAccelerometerCalibrator(
19184                     -gravityNorm, ba, ma, this);
19185             fail("IllegalArgumentException expected but not thrown");
19186         } catch (final IllegalArgumentException ignore) {
19187         }
19188         try {
19189             calibrator = new KnownGravityNormAccelerometerCalibrator(
19190                     gravityNorm, new Matrix(1, 1), ma,
19191                     this);
19192             fail("IllegalArgumentException expected but not thrown");
19193         } catch (final IllegalArgumentException ignore) {
19194         }
19195         try {
19196             calibrator = new KnownGravityNormAccelerometerCalibrator(
19197                     gravityNorm, new Matrix(1, 3), ma,
19198                     this);
19199             fail("IllegalArgumentException expected but not thrown");
19200         } catch (final IllegalArgumentException ignore) {
19201         }
19202         try {
19203             calibrator = new KnownGravityNormAccelerometerCalibrator(
19204                     gravityNorm, ba, new Matrix(1, 3), this);
19205             fail("IllegalArgumentException expected but not thrown");
19206         } catch (final IllegalArgumentException ignore) {
19207         }
19208         try {
19209             calibrator = new KnownGravityNormAccelerometerCalibrator(
19210                     gravityNorm, ba, new Matrix(3, 1), this);
19211             fail("IllegalArgumentException expected but not thrown");
19212         } catch (final IllegalArgumentException ignore) {
19213         }
19214         assertNull(calibrator);
19215     }
19216 
19217     @Test
19218     public void testConstructor151() throws WrongSizeException {
19219         final Collection<StandardDeviationBodyKinematics> measurements =
19220                 Collections.emptyList();
19221 
19222         final Matrix ba = generateBa();
19223         final double[] bias = ba.getBuffer();
19224         final double biasX = ba.getElementAtIndex(0);
19225         final double biasY = ba.getElementAtIndex(1);
19226         final double biasZ = ba.getElementAtIndex(2);
19227 
19228         final Matrix ma = generateMaCommonAxis();
19229         final double sx = ma.getElementAt(0, 0);
19230         final double sy = ma.getElementAt(1, 1);
19231         final double sz = ma.getElementAt(2, 2);
19232         final double mxy = ma.getElementAt(0, 1);
19233         final double mxz = ma.getElementAt(0, 2);
19234         final double myx = ma.getElementAt(1, 0);
19235         final double myz = ma.getElementAt(1, 2);
19236         final double mzx = ma.getElementAt(2, 0);
19237         final double mzy = ma.getElementAt(2, 1);
19238 
19239         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19240         final double latitude = Math.toRadians(
19241                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19242         final double longitude = Math.toRadians(
19243                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19244         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19245         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19246         final NEDVelocity nedVelocity = new NEDVelocity();
19247         final ECEFPosition ecefPosition = new ECEFPosition();
19248         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19249         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19250                 ecefPosition, ecefVelocity);
19251         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19252                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19253         final double gravityNorm = gravity.getNorm();
19254 
19255         KnownGravityNormAccelerometerCalibrator calibrator =
19256                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
19257                         measurements, ba, ma);
19258 
19259         // check default values
19260         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19261         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19262         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19263         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19264         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19265         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19266         final Acceleration bx2 = new Acceleration(0.0,
19267                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19268         calibrator.getInitialBiasXAsAcceleration(bx2);
19269         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19270         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19271         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19272         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19273         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19274         final Acceleration by2 = new Acceleration(0.0,
19275                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19276         calibrator.getInitialBiasYAsAcceleration(by2);
19277         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19278         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19279         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19280         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19281         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19282         final Acceleration bz2 = new Acceleration(0.0,
19283                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19284         calibrator.getInitialBiasZAsAcceleration(bz2);
19285         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19286         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19287         assertEquals(calibrator.getInitialSx(), sx, 0.0);
19288         assertEquals(calibrator.getInitialSy(), sy, 0.0);
19289         assertEquals(calibrator.getInitialSz(), sz, 0.0);
19290         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19291         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19292         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19293         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19294         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19295         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19296         final double[] bias1 = calibrator.getInitialBias();
19297         assertArrayEquals(bias1, bias, 0.0);
19298         final double[] bias2 = new double[3];
19299         calibrator.getInitialBias(bias2);
19300         assertArrayEquals(bias1, bias2, 0.0);
19301         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19302         assertEquals(b1, ba);
19303         final Matrix b2 = new Matrix(3, 1);
19304         calibrator.getInitialBiasAsMatrix(b2);
19305         assertEquals(b1, b2);
19306         final Matrix ma1 = new Matrix(3, 3);
19307         ma1.setSubmatrix(0, 0,
19308                 2, 2,
19309                 new double[]{sx, myx, mzx,
19310                         mxy, sy, mzy,
19311                         mxz, myz, sz});
19312         assertEquals(calibrator.getInitialMa(), ma1);
19313         final Matrix ma2 = new Matrix(3, 3);
19314         calibrator.getInitialMa(ma2);
19315         assertEquals(ma1, ma2);
19316         assertSame(calibrator.getMeasurements(), measurements);
19317         assertFalse(calibrator.isCommonAxisUsed());
19318         assertNull(calibrator.getListener());
19319         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19320         assertFalse(calibrator.isReady());
19321         assertFalse(calibrator.isRunning());
19322         assertNull(calibrator.getEstimatedBiases());
19323         assertFalse(calibrator.getEstimatedBiases(null));
19324         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19325         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19326         assertNull(calibrator.getEstimatedBiasFx());
19327         assertNull(calibrator.getEstimatedBiasFy());
19328         assertNull(calibrator.getEstimatedBiasFz());
19329         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19330         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19331         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19332         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19333         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19334         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19335         assertNull(calibrator.getEstimatedMa());
19336         assertNull(calibrator.getEstimatedSx());
19337         assertNull(calibrator.getEstimatedSy());
19338         assertNull(calibrator.getEstimatedSz());
19339         assertNull(calibrator.getEstimatedMxy());
19340         assertNull(calibrator.getEstimatedMxz());
19341         assertNull(calibrator.getEstimatedMyx());
19342         assertNull(calibrator.getEstimatedMyz());
19343         assertNull(calibrator.getEstimatedMzx());
19344         assertNull(calibrator.getEstimatedMzy());
19345         assertNull(calibrator.getEstimatedCovariance());
19346         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19347         assertNotNull(calibrator.getGroundTruthGravityNorm());
19348         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19349         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19350         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19351                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19352         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19353         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19354         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19355 
19356         // Force IllegalArgumentException
19357         calibrator = null;
19358         try {
19359             calibrator = new KnownGravityNormAccelerometerCalibrator(
19360                     -gravityNorm, measurements, ba, ma);
19361             fail("IllegalArgumentException expected but not thrown");
19362         } catch (final IllegalArgumentException ignore) {
19363         }
19364         try {
19365             calibrator = new KnownGravityNormAccelerometerCalibrator(
19366                     gravityNorm, measurements,
19367                     new Matrix(1, 1), ma);
19368             fail("IllegalArgumentException expected but not thrown");
19369         } catch (final IllegalArgumentException ignore) {
19370         }
19371         try {
19372             calibrator = new KnownGravityNormAccelerometerCalibrator(
19373                     gravityNorm, measurements,
19374                     new Matrix(1, 3), ma);
19375             fail("IllegalArgumentException expected but not thrown");
19376         } catch (final IllegalArgumentException ignore) {
19377         }
19378         try {
19379             calibrator = new KnownGravityNormAccelerometerCalibrator(
19380                     gravityNorm, measurements, ba,
19381                     new Matrix(1, 3));
19382             fail("IllegalArgumentException expected but not thrown");
19383         } catch (final IllegalArgumentException ignore) {
19384         }
19385         try {
19386             calibrator = new KnownGravityNormAccelerometerCalibrator(
19387                     gravityNorm, measurements, ba,
19388                     new Matrix(3, 1));
19389             fail("IllegalArgumentException expected but not thrown");
19390         } catch (final IllegalArgumentException ignore) {
19391         }
19392         assertNull(calibrator);
19393     }
19394 
19395     @Test
19396     public void testConstructor152() throws WrongSizeException {
19397         final Collection<StandardDeviationBodyKinematics> measurements =
19398                 Collections.emptyList();
19399 
19400         final Matrix ba = generateBa();
19401         final double[] bias = ba.getBuffer();
19402         final double biasX = ba.getElementAtIndex(0);
19403         final double biasY = ba.getElementAtIndex(1);
19404         final double biasZ = ba.getElementAtIndex(2);
19405 
19406         final Matrix ma = generateMaCommonAxis();
19407         final double sx = ma.getElementAt(0, 0);
19408         final double sy = ma.getElementAt(1, 1);
19409         final double sz = ma.getElementAt(2, 2);
19410         final double mxy = ma.getElementAt(0, 1);
19411         final double mxz = ma.getElementAt(0, 2);
19412         final double myx = ma.getElementAt(1, 0);
19413         final double myz = ma.getElementAt(1, 2);
19414         final double mzx = ma.getElementAt(2, 0);
19415         final double mzy = ma.getElementAt(2, 1);
19416 
19417         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19418         final double latitude = Math.toRadians(
19419                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19420         final double longitude = Math.toRadians(
19421                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19422         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19423         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19424         final NEDVelocity nedVelocity = new NEDVelocity();
19425         final ECEFPosition ecefPosition = new ECEFPosition();
19426         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19427         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19428                 ecefPosition, ecefVelocity);
19429         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19430                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19431         final double gravityNorm = gravity.getNorm();
19432 
19433         KnownGravityNormAccelerometerCalibrator calibrator =
19434                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
19435                         measurements, ba, ma, this);
19436 
19437         // check default values
19438         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19439         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19440         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19441         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19442         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19443         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19444         final Acceleration bx2 = new Acceleration(0.0,
19445                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19446         calibrator.getInitialBiasXAsAcceleration(bx2);
19447         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19448         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19449         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19450         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19451         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19452         final Acceleration by2 = new Acceleration(0.0,
19453                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19454         calibrator.getInitialBiasYAsAcceleration(by2);
19455         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19456         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19457         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19458         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19459         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19460         final Acceleration bz2 = new Acceleration(0.0,
19461                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19462         calibrator.getInitialBiasZAsAcceleration(bz2);
19463         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19464         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19465         assertEquals(calibrator.getInitialSx(), sx, 0.0);
19466         assertEquals(calibrator.getInitialSy(), sy, 0.0);
19467         assertEquals(calibrator.getInitialSz(), sz, 0.0);
19468         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19469         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19470         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19471         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19472         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19473         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19474         final double[] bias1 = calibrator.getInitialBias();
19475         assertArrayEquals(bias1, bias, 0.0);
19476         final double[] bias2 = new double[3];
19477         calibrator.getInitialBias(bias2);
19478         assertArrayEquals(bias1, bias2, 0.0);
19479         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19480         assertEquals(b1, ba);
19481         final Matrix b2 = new Matrix(3, 1);
19482         calibrator.getInitialBiasAsMatrix(b2);
19483         assertEquals(b1, b2);
19484         final Matrix ma1 = new Matrix(3, 3);
19485         ma1.setSubmatrix(0, 0,
19486                 2, 2,
19487                 new double[]{sx, myx, mzx,
19488                         mxy, sy, mzy,
19489                         mxz, myz, sz});
19490         assertEquals(calibrator.getInitialMa(), ma1);
19491         final Matrix ma2 = new Matrix(3, 3);
19492         calibrator.getInitialMa(ma2);
19493         assertEquals(ma1, ma2);
19494         assertSame(calibrator.getMeasurements(), measurements);
19495         assertFalse(calibrator.isCommonAxisUsed());
19496         assertSame(calibrator.getListener(), this);
19497         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19498         assertFalse(calibrator.isReady());
19499         assertFalse(calibrator.isRunning());
19500         assertNull(calibrator.getEstimatedBiases());
19501         assertFalse(calibrator.getEstimatedBiases(null));
19502         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19503         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19504         assertNull(calibrator.getEstimatedBiasFx());
19505         assertNull(calibrator.getEstimatedBiasFy());
19506         assertNull(calibrator.getEstimatedBiasFz());
19507         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19508         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19509         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19510         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19511         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19512         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19513         assertNull(calibrator.getEstimatedMa());
19514         assertNull(calibrator.getEstimatedSx());
19515         assertNull(calibrator.getEstimatedSy());
19516         assertNull(calibrator.getEstimatedSz());
19517         assertNull(calibrator.getEstimatedMxy());
19518         assertNull(calibrator.getEstimatedMxz());
19519         assertNull(calibrator.getEstimatedMyx());
19520         assertNull(calibrator.getEstimatedMyz());
19521         assertNull(calibrator.getEstimatedMzx());
19522         assertNull(calibrator.getEstimatedMzy());
19523         assertNull(calibrator.getEstimatedCovariance());
19524         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19525         assertNotNull(calibrator.getGroundTruthGravityNorm());
19526         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19527         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19528         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19529                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19530         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19531         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19532         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19533 
19534         // Force IllegalArgumentException
19535         calibrator = null;
19536         try {
19537             calibrator = new KnownGravityNormAccelerometerCalibrator(
19538                     -gravityNorm, measurements, ba, ma, this);
19539             fail("IllegalArgumentException expected but not thrown");
19540         } catch (final IllegalArgumentException ignore) {
19541         }
19542         try {
19543             calibrator = new KnownGravityNormAccelerometerCalibrator(
19544                     gravityNorm, measurements,
19545                     new Matrix(1, 1), ma, this);
19546             fail("IllegalArgumentException expected but not thrown");
19547         } catch (final IllegalArgumentException ignore) {
19548         }
19549         try {
19550             calibrator = new KnownGravityNormAccelerometerCalibrator(
19551                     gravityNorm, measurements,
19552                     new Matrix(1, 3), ma, this);
19553             fail("IllegalArgumentException expected but not thrown");
19554         } catch (final IllegalArgumentException ignore) {
19555         }
19556         try {
19557             calibrator = new KnownGravityNormAccelerometerCalibrator(
19558                     gravityNorm, measurements, ba,
19559                     new Matrix(1, 3), this);
19560             fail("IllegalArgumentException expected but not thrown");
19561         } catch (final IllegalArgumentException ignore) {
19562         }
19563         try {
19564             calibrator = new KnownGravityNormAccelerometerCalibrator(
19565                     gravityNorm, measurements, ba,
19566                     new Matrix(3, 1), this);
19567             fail("IllegalArgumentException expected but not thrown");
19568         } catch (final IllegalArgumentException ignore) {
19569         }
19570         assertNull(calibrator);
19571     }
19572 
19573     @Test
19574     public void testConstructor153() throws WrongSizeException {
19575         final Matrix ba = generateBa();
19576         final double[] bias = ba.getBuffer();
19577         final double biasX = ba.getElementAtIndex(0);
19578         final double biasY = ba.getElementAtIndex(1);
19579         final double biasZ = ba.getElementAtIndex(2);
19580 
19581         final Matrix ma = generateMaCommonAxis();
19582         final double sx = ma.getElementAt(0, 0);
19583         final double sy = ma.getElementAt(1, 1);
19584         final double sz = ma.getElementAt(2, 2);
19585         final double mxy = ma.getElementAt(0, 1);
19586         final double mxz = ma.getElementAt(0, 2);
19587         final double myx = ma.getElementAt(1, 0);
19588         final double myz = ma.getElementAt(1, 2);
19589         final double mzx = ma.getElementAt(2, 0);
19590         final double mzy = ma.getElementAt(2, 1);
19591 
19592         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19593         final double latitude = Math.toRadians(
19594                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19595         final double longitude = Math.toRadians(
19596                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19597         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19598         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19599         final NEDVelocity nedVelocity = new NEDVelocity();
19600         final ECEFPosition ecefPosition = new ECEFPosition();
19601         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19602         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19603                 ecefPosition, ecefVelocity);
19604         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19605                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19606         final double gravityNorm = gravity.getNorm();
19607 
19608         KnownGravityNormAccelerometerCalibrator calibrator =
19609                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
19610                         true, ba, ma);
19611 
19612         // check default values
19613         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19614         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19615         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19616         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19617         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19618         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19619         final Acceleration bx2 = new Acceleration(0.0,
19620                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19621         calibrator.getInitialBiasXAsAcceleration(bx2);
19622         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19623         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19624         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19625         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19626         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19627         final Acceleration by2 = new Acceleration(0.0,
19628                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19629         calibrator.getInitialBiasYAsAcceleration(by2);
19630         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19631         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19632         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19633         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19634         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19635         final Acceleration bz2 = new Acceleration(0.0,
19636                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19637         calibrator.getInitialBiasZAsAcceleration(bz2);
19638         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19639         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19640         assertEquals(calibrator.getInitialSx(), sx, 0.0);
19641         assertEquals(calibrator.getInitialSy(), sy, 0.0);
19642         assertEquals(calibrator.getInitialSz(), sz, 0.0);
19643         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19644         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19645         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19646         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19647         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19648         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19649         final double[] bias1 = calibrator.getInitialBias();
19650         assertArrayEquals(bias1, bias, 0.0);
19651         final double[] bias2 = new double[3];
19652         calibrator.getInitialBias(bias2);
19653         assertArrayEquals(bias1, bias2, 0.0);
19654         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19655         assertEquals(b1, ba);
19656         final Matrix b2 = new Matrix(3, 1);
19657         calibrator.getInitialBiasAsMatrix(b2);
19658         assertEquals(b1, b2);
19659         final Matrix ma1 = new Matrix(3, 3);
19660         ma1.setSubmatrix(0, 0,
19661                 2, 2,
19662                 new double[]{sx, myx, mzx,
19663                         mxy, sy, mzy,
19664                         mxz, myz, sz});
19665         assertEquals(calibrator.getInitialMa(), ma1);
19666         final Matrix ma2 = new Matrix(3, 3);
19667         calibrator.getInitialMa(ma2);
19668         assertEquals(ma1, ma2);
19669         assertNull(calibrator.getMeasurements());
19670         assertTrue(calibrator.isCommonAxisUsed());
19671         assertNull(calibrator.getListener());
19672         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19673         assertFalse(calibrator.isReady());
19674         assertFalse(calibrator.isRunning());
19675         assertNull(calibrator.getEstimatedBiases());
19676         assertFalse(calibrator.getEstimatedBiases(null));
19677         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19678         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19679         assertNull(calibrator.getEstimatedBiasFx());
19680         assertNull(calibrator.getEstimatedBiasFy());
19681         assertNull(calibrator.getEstimatedBiasFz());
19682         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19683         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19684         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19685         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19686         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19687         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19688         assertNull(calibrator.getEstimatedMa());
19689         assertNull(calibrator.getEstimatedSx());
19690         assertNull(calibrator.getEstimatedSy());
19691         assertNull(calibrator.getEstimatedSz());
19692         assertNull(calibrator.getEstimatedMxy());
19693         assertNull(calibrator.getEstimatedMxz());
19694         assertNull(calibrator.getEstimatedMyx());
19695         assertNull(calibrator.getEstimatedMyz());
19696         assertNull(calibrator.getEstimatedMzx());
19697         assertNull(calibrator.getEstimatedMzy());
19698         assertNull(calibrator.getEstimatedCovariance());
19699         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19700         assertNotNull(calibrator.getGroundTruthGravityNorm());
19701         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19702         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19703         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19704                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19705         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19706         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19707         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19708 
19709         // Force IllegalArgumentException
19710         calibrator = null;
19711         try {
19712             calibrator = new KnownGravityNormAccelerometerCalibrator(
19713                     -gravityNorm, true, ba, ma);
19714             fail("IllegalArgumentException expected but not thrown");
19715         } catch (final IllegalArgumentException ignore) {
19716         }
19717         try {
19718             calibrator = new KnownGravityNormAccelerometerCalibrator(
19719                     gravityNorm, true,
19720                     new Matrix(1, 1), ma);
19721             fail("IllegalArgumentException expected but not thrown");
19722         } catch (final IllegalArgumentException ignore) {
19723         }
19724         try {
19725             calibrator = new KnownGravityNormAccelerometerCalibrator(
19726                     gravityNorm, true,
19727                     new Matrix(1, 3), ma);
19728             fail("IllegalArgumentException expected but not thrown");
19729         } catch (final IllegalArgumentException ignore) {
19730         }
19731         try {
19732             calibrator = new KnownGravityNormAccelerometerCalibrator(
19733                     gravityNorm, true, ba,
19734                     new Matrix(1, 3));
19735             fail("IllegalArgumentException expected but not thrown");
19736         } catch (final IllegalArgumentException ignore) {
19737         }
19738         try {
19739             calibrator = new KnownGravityNormAccelerometerCalibrator(
19740                     gravityNorm, true, ba,
19741                     new Matrix(3, 1));
19742             fail("IllegalArgumentException expected but not thrown");
19743         } catch (final IllegalArgumentException ignore) {
19744         }
19745         assertNull(calibrator);
19746     }
19747 
19748     @Test
19749     public void testConstructor154() throws WrongSizeException {
19750         final Matrix ba = generateBa();
19751         final double[] bias = ba.getBuffer();
19752         final double biasX = ba.getElementAtIndex(0);
19753         final double biasY = ba.getElementAtIndex(1);
19754         final double biasZ = ba.getElementAtIndex(2);
19755 
19756         final Matrix ma = generateMaCommonAxis();
19757         final double sx = ma.getElementAt(0, 0);
19758         final double sy = ma.getElementAt(1, 1);
19759         final double sz = ma.getElementAt(2, 2);
19760         final double mxy = ma.getElementAt(0, 1);
19761         final double mxz = ma.getElementAt(0, 2);
19762         final double myx = ma.getElementAt(1, 0);
19763         final double myz = ma.getElementAt(1, 2);
19764         final double mzx = ma.getElementAt(2, 0);
19765         final double mzy = ma.getElementAt(2, 1);
19766 
19767         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19768         final double latitude = Math.toRadians(
19769                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19770         final double longitude = Math.toRadians(
19771                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19772         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19773         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19774         final NEDVelocity nedVelocity = new NEDVelocity();
19775         final ECEFPosition ecefPosition = new ECEFPosition();
19776         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19777         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19778                 ecefPosition, ecefVelocity);
19779         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19780                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19781         final double gravityNorm = gravity.getNorm();
19782 
19783         KnownGravityNormAccelerometerCalibrator calibrator =
19784                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
19785                         true, ba, ma, this);
19786 
19787         // check default values
19788         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19789         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19790         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19791         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19792         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19793         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19794         final Acceleration bx2 = new Acceleration(0.0,
19795                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19796         calibrator.getInitialBiasXAsAcceleration(bx2);
19797         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19798         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19799         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19800         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19801         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19802         final Acceleration by2 = new Acceleration(0.0,
19803                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19804         calibrator.getInitialBiasYAsAcceleration(by2);
19805         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19806         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19807         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19808         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19809         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19810         final Acceleration bz2 = new Acceleration(0.0,
19811                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19812         calibrator.getInitialBiasZAsAcceleration(bz2);
19813         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19814         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19815         assertEquals(calibrator.getInitialSx(), sx, 0.0);
19816         assertEquals(calibrator.getInitialSy(), sy, 0.0);
19817         assertEquals(calibrator.getInitialSz(), sz, 0.0);
19818         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19819         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19820         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19821         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19822         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19823         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19824         final double[] bias1 = calibrator.getInitialBias();
19825         assertArrayEquals(bias1, bias, 0.0);
19826         final double[] bias2 = new double[3];
19827         calibrator.getInitialBias(bias2);
19828         assertArrayEquals(bias1, bias2, 0.0);
19829         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19830         assertEquals(b1, ba);
19831         final Matrix b2 = new Matrix(3, 1);
19832         calibrator.getInitialBiasAsMatrix(b2);
19833         assertEquals(b1, b2);
19834         final Matrix ma1 = new Matrix(3, 3);
19835         ma1.setSubmatrix(0, 0,
19836                 2, 2,
19837                 new double[]{sx, myx, mzx,
19838                         mxy, sy, mzy,
19839                         mxz, myz, sz});
19840         assertEquals(calibrator.getInitialMa(), ma1);
19841         final Matrix ma2 = new Matrix(3, 3);
19842         calibrator.getInitialMa(ma2);
19843         assertEquals(ma1, ma2);
19844         assertNull(calibrator.getMeasurements());
19845         assertTrue(calibrator.isCommonAxisUsed());
19846         assertSame(calibrator.getListener(), this);
19847         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19848         assertFalse(calibrator.isReady());
19849         assertFalse(calibrator.isRunning());
19850         assertNull(calibrator.getEstimatedBiases());
19851         assertFalse(calibrator.getEstimatedBiases(null));
19852         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19853         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19854         assertNull(calibrator.getEstimatedBiasFx());
19855         assertNull(calibrator.getEstimatedBiasFy());
19856         assertNull(calibrator.getEstimatedBiasFz());
19857         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19858         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19859         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19860         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19861         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19862         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19863         assertNull(calibrator.getEstimatedMa());
19864         assertNull(calibrator.getEstimatedSx());
19865         assertNull(calibrator.getEstimatedSy());
19866         assertNull(calibrator.getEstimatedSz());
19867         assertNull(calibrator.getEstimatedMxy());
19868         assertNull(calibrator.getEstimatedMxz());
19869         assertNull(calibrator.getEstimatedMyx());
19870         assertNull(calibrator.getEstimatedMyz());
19871         assertNull(calibrator.getEstimatedMzx());
19872         assertNull(calibrator.getEstimatedMzy());
19873         assertNull(calibrator.getEstimatedCovariance());
19874         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19875         assertNotNull(calibrator.getGroundTruthGravityNorm());
19876         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19877         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19878         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19879                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19880         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19881         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19882         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19883 
19884         // Force IllegalArgumentException
19885         calibrator = null;
19886         try {
19887             calibrator = new KnownGravityNormAccelerometerCalibrator(
19888                     -gravityNorm, true, ba, ma, this);
19889             fail("IllegalArgumentException expected but not thrown");
19890         } catch (final IllegalArgumentException ignore) {
19891         }
19892         try {
19893             calibrator = new KnownGravityNormAccelerometerCalibrator(
19894                     gravityNorm, true, new Matrix(1, 1),
19895                     ma, this);
19896             fail("IllegalArgumentException expected but not thrown");
19897         } catch (final IllegalArgumentException ignore) {
19898         }
19899         try {
19900             calibrator = new KnownGravityNormAccelerometerCalibrator(
19901                     gravityNorm, true, new Matrix(1, 3),
19902                     ma, this);
19903             fail("IllegalArgumentException expected but not thrown");
19904         } catch (final IllegalArgumentException ignore) {
19905         }
19906         try {
19907             calibrator = new KnownGravityNormAccelerometerCalibrator(
19908                     gravityNorm, true, ba, new Matrix(1, 3),
19909                     this);
19910             fail("IllegalArgumentException expected but not thrown");
19911         } catch (final IllegalArgumentException ignore) {
19912         }
19913         try {
19914             calibrator = new KnownGravityNormAccelerometerCalibrator(
19915                     gravityNorm, true, ba, new Matrix(3, 1),
19916                     this);
19917             fail("IllegalArgumentException expected but not thrown");
19918         } catch (final IllegalArgumentException ignore) {
19919         }
19920         assertNull(calibrator);
19921     }
19922 
19923     @Test
19924     public void testConstructor155() throws WrongSizeException {
19925         final Collection<StandardDeviationBodyKinematics> measurements =
19926                 Collections.emptyList();
19927 
19928         final Matrix ba = generateBa();
19929         final double[] bias = ba.getBuffer();
19930         final double biasX = ba.getElementAtIndex(0);
19931         final double biasY = ba.getElementAtIndex(1);
19932         final double biasZ = ba.getElementAtIndex(2);
19933 
19934         final Matrix ma = generateMaCommonAxis();
19935         final double sx = ma.getElementAt(0, 0);
19936         final double sy = ma.getElementAt(1, 1);
19937         final double sz = ma.getElementAt(2, 2);
19938         final double mxy = ma.getElementAt(0, 1);
19939         final double mxz = ma.getElementAt(0, 2);
19940         final double myx = ma.getElementAt(1, 0);
19941         final double myz = ma.getElementAt(1, 2);
19942         final double mzx = ma.getElementAt(2, 0);
19943         final double mzy = ma.getElementAt(2, 1);
19944 
19945         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19946         final double latitude = Math.toRadians(
19947                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19948         final double longitude = Math.toRadians(
19949                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19950         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19951         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19952         final NEDVelocity nedVelocity = new NEDVelocity();
19953         final ECEFPosition ecefPosition = new ECEFPosition();
19954         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19955         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19956                 ecefPosition, ecefVelocity);
19957         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19958                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19959         final double gravityNorm = gravity.getNorm();
19960 
19961         KnownGravityNormAccelerometerCalibrator calibrator =
19962                 new KnownGravityNormAccelerometerCalibrator(
19963                         gravityNorm, measurements,
19964                         true, ba, ma);
19965 
19966         // check default values
19967         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19968         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19969         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19970         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19971         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19972         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19973         final Acceleration bx2 = new Acceleration(0.0,
19974                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19975         calibrator.getInitialBiasXAsAcceleration(bx2);
19976         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19977         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19978         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19979         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19980         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19981         final Acceleration by2 = new Acceleration(0.0,
19982                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19983         calibrator.getInitialBiasYAsAcceleration(by2);
19984         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19985         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19986         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19987         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19988         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19989         final Acceleration bz2 = new Acceleration(0.0,
19990                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19991         calibrator.getInitialBiasZAsAcceleration(bz2);
19992         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19993         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19994         assertEquals(calibrator.getInitialSx(), sx, 0.0);
19995         assertEquals(calibrator.getInitialSy(), sy, 0.0);
19996         assertEquals(calibrator.getInitialSz(), sz, 0.0);
19997         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19998         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19999         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
20000         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
20001         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
20002         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
20003         final double[] bias1 = calibrator.getInitialBias();
20004         assertArrayEquals(bias1, bias, 0.0);
20005         final double[] bias2 = new double[3];
20006         calibrator.getInitialBias(bias2);
20007         assertArrayEquals(bias1, bias2, 0.0);
20008         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20009         assertEquals(b1, ba);
20010         final Matrix b2 = new Matrix(3, 1);
20011         calibrator.getInitialBiasAsMatrix(b2);
20012         assertEquals(b1, b2);
20013         final Matrix ma1 = new Matrix(3, 3);
20014         ma1.setSubmatrix(0, 0,
20015                 2, 2,
20016                 new double[]{sx, myx, mzx,
20017                         mxy, sy, mzy,
20018                         mxz, myz, sz});
20019         assertEquals(calibrator.getInitialMa(), ma1);
20020         final Matrix ma2 = new Matrix(3, 3);
20021         calibrator.getInitialMa(ma2);
20022         assertEquals(ma1, ma2);
20023         assertSame(calibrator.getMeasurements(), measurements);
20024         assertTrue(calibrator.isCommonAxisUsed());
20025         assertNull(calibrator.getListener());
20026         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20027         assertFalse(calibrator.isReady());
20028         assertFalse(calibrator.isRunning());
20029         assertNull(calibrator.getEstimatedBiases());
20030         assertFalse(calibrator.getEstimatedBiases(null));
20031         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20032         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20033         assertNull(calibrator.getEstimatedBiasFx());
20034         assertNull(calibrator.getEstimatedBiasFy());
20035         assertNull(calibrator.getEstimatedBiasFz());
20036         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20037         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20038         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20039         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20040         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20041         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20042         assertNull(calibrator.getEstimatedMa());
20043         assertNull(calibrator.getEstimatedSx());
20044         assertNull(calibrator.getEstimatedSy());
20045         assertNull(calibrator.getEstimatedSz());
20046         assertNull(calibrator.getEstimatedMxy());
20047         assertNull(calibrator.getEstimatedMxz());
20048         assertNull(calibrator.getEstimatedMyx());
20049         assertNull(calibrator.getEstimatedMyz());
20050         assertNull(calibrator.getEstimatedMzx());
20051         assertNull(calibrator.getEstimatedMzy());
20052         assertNull(calibrator.getEstimatedCovariance());
20053         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20054         assertNotNull(calibrator.getGroundTruthGravityNorm());
20055         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20056         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20057         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20058                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20059         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20060         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20061         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20062 
20063         // Force IllegalArgumentException
20064         calibrator = null;
20065         try {
20066             calibrator = new KnownGravityNormAccelerometerCalibrator(
20067                     -gravityNorm, measurements,
20068                     true, ba, ma);
20069             fail("IllegalArgumentException expected but not thrown");
20070         } catch (final IllegalArgumentException ignore) {
20071         }
20072         try {
20073             calibrator = new KnownGravityNormAccelerometerCalibrator(
20074                     gravityNorm, measurements, true,
20075                     new Matrix(1, 1), ma);
20076             fail("IllegalArgumentException expected but not thrown");
20077         } catch (final IllegalArgumentException ignore) {
20078         }
20079         try {
20080             calibrator = new KnownGravityNormAccelerometerCalibrator(
20081                     gravityNorm, measurements, true,
20082                     new Matrix(1, 3), ma);
20083             fail("IllegalArgumentException expected but not thrown");
20084         } catch (final IllegalArgumentException ignore) {
20085         }
20086         try {
20087             calibrator = new KnownGravityNormAccelerometerCalibrator(
20088                     gravityNorm, measurements, true,
20089                     ba, new Matrix(1, 3));
20090             fail("IllegalArgumentException expected but not thrown");
20091         } catch (final IllegalArgumentException ignore) {
20092         }
20093         try {
20094             calibrator = new KnownGravityNormAccelerometerCalibrator(
20095                     gravityNorm, measurements, true,
20096                     ba, new Matrix(3, 1));
20097             fail("IllegalArgumentException expected but not thrown");
20098         } catch (final IllegalArgumentException ignore) {
20099         }
20100         assertNull(calibrator);
20101     }
20102 
20103     @Test
20104     public void testConstructor156() throws WrongSizeException {
20105         final Collection<StandardDeviationBodyKinematics> measurements =
20106                 Collections.emptyList();
20107 
20108         final Matrix ba = generateBa();
20109         final double[] bias = ba.getBuffer();
20110         final double biasX = ba.getElementAtIndex(0);
20111         final double biasY = ba.getElementAtIndex(1);
20112         final double biasZ = ba.getElementAtIndex(2);
20113 
20114         final Matrix ma = generateMaCommonAxis();
20115         final double sx = ma.getElementAt(0, 0);
20116         final double sy = ma.getElementAt(1, 1);
20117         final double sz = ma.getElementAt(2, 2);
20118         final double mxy = ma.getElementAt(0, 1);
20119         final double mxz = ma.getElementAt(0, 2);
20120         final double myx = ma.getElementAt(1, 0);
20121         final double myz = ma.getElementAt(1, 2);
20122         final double mzx = ma.getElementAt(2, 0);
20123         final double mzy = ma.getElementAt(2, 1);
20124 
20125         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20126         final double latitude = Math.toRadians(
20127                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20128         final double longitude = Math.toRadians(
20129                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20130         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20131         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20132         final NEDVelocity nedVelocity = new NEDVelocity();
20133         final ECEFPosition ecefPosition = new ECEFPosition();
20134         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20135         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20136                 ecefPosition, ecefVelocity);
20137         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20138                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20139         final double gravityNorm = gravity.getNorm();
20140 
20141         KnownGravityNormAccelerometerCalibrator calibrator =
20142                 new KnownGravityNormAccelerometerCalibrator(
20143                         gravityNorm, measurements,
20144                         true, ba, ma, this);
20145 
20146         // check default values
20147         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20148         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20149         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20150         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20151         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20152         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20153         final Acceleration bx2 = new Acceleration(0.0,
20154                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20155         calibrator.getInitialBiasXAsAcceleration(bx2);
20156         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20157         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20158         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20159         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20160         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20161         final Acceleration by2 = new Acceleration(0.0,
20162                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20163         calibrator.getInitialBiasYAsAcceleration(by2);
20164         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20165         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20166         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20167         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
20168         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20169         final Acceleration bz2 = new Acceleration(0.0,
20170                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20171         calibrator.getInitialBiasZAsAcceleration(bz2);
20172         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
20173         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20174         assertEquals(calibrator.getInitialSx(), sx, 0.0);
20175         assertEquals(calibrator.getInitialSy(), sy, 0.0);
20176         assertEquals(calibrator.getInitialSz(), sz, 0.0);
20177         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
20178         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
20179         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
20180         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
20181         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
20182         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
20183         final double[] bias1 = calibrator.getInitialBias();
20184         assertArrayEquals(bias1, bias, 0.0);
20185         final double[] bias2 = new double[3];
20186         calibrator.getInitialBias(bias2);
20187         assertArrayEquals(bias1, bias2, 0.0);
20188         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20189         assertEquals(b1, ba);
20190         final Matrix b2 = new Matrix(3, 1);
20191         calibrator.getInitialBiasAsMatrix(b2);
20192         assertEquals(b1, b2);
20193         final Matrix ma1 = new Matrix(3, 3);
20194         ma1.setSubmatrix(0, 0,
20195                 2, 2,
20196                 new double[]{sx, myx, mzx,
20197                         mxy, sy, mzy,
20198                         mxz, myz, sz});
20199         assertEquals(calibrator.getInitialMa(), ma1);
20200         final Matrix ma2 = new Matrix(3, 3);
20201         calibrator.getInitialMa(ma2);
20202         assertEquals(ma1, ma2);
20203         assertSame(calibrator.getMeasurements(), measurements);
20204         assertTrue(calibrator.isCommonAxisUsed());
20205         assertSame(calibrator.getListener(), this);
20206         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20207         assertFalse(calibrator.isReady());
20208         assertFalse(calibrator.isRunning());
20209         assertNull(calibrator.getEstimatedBiases());
20210         assertFalse(calibrator.getEstimatedBiases(null));
20211         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20212         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20213         assertNull(calibrator.getEstimatedBiasFx());
20214         assertNull(calibrator.getEstimatedBiasFy());
20215         assertNull(calibrator.getEstimatedBiasFz());
20216         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20217         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20218         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20219         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20220         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20221         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20222         assertNull(calibrator.getEstimatedMa());
20223         assertNull(calibrator.getEstimatedSx());
20224         assertNull(calibrator.getEstimatedSy());
20225         assertNull(calibrator.getEstimatedSz());
20226         assertNull(calibrator.getEstimatedMxy());
20227         assertNull(calibrator.getEstimatedMxz());
20228         assertNull(calibrator.getEstimatedMyx());
20229         assertNull(calibrator.getEstimatedMyz());
20230         assertNull(calibrator.getEstimatedMzx());
20231         assertNull(calibrator.getEstimatedMzy());
20232         assertNull(calibrator.getEstimatedCovariance());
20233         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20234         assertNotNull(calibrator.getGroundTruthGravityNorm());
20235         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20236         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20237         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20238                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20239         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20240         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20241         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20242 
20243         // Force IllegalArgumentException
20244         calibrator = null;
20245         try {
20246             calibrator = new KnownGravityNormAccelerometerCalibrator(
20247                     -gravityNorm, measurements,
20248                     true, ba, ma, this);
20249             fail("IllegalArgumentException expected but not thrown");
20250         } catch (final IllegalArgumentException ignore) {
20251         }
20252         try {
20253             calibrator = new KnownGravityNormAccelerometerCalibrator(
20254                     gravityNorm, measurements, true,
20255                     new Matrix(1, 1), ma, this);
20256             fail("IllegalArgumentException expected but not thrown");
20257         } catch (final IllegalArgumentException ignore) {
20258         }
20259         try {
20260             calibrator = new KnownGravityNormAccelerometerCalibrator(
20261                     gravityNorm, measurements, true,
20262                     new Matrix(1, 3), ma, this);
20263             fail("IllegalArgumentException expected but not thrown");
20264         } catch (final IllegalArgumentException ignore) {
20265         }
20266         try {
20267             calibrator = new KnownGravityNormAccelerometerCalibrator(
20268                     gravityNorm, measurements, true,
20269                     ba, new Matrix(1, 3), this);
20270             fail("IllegalArgumentException expected but not thrown");
20271         } catch (final IllegalArgumentException ignore) {
20272         }
20273         try {
20274             calibrator = new KnownGravityNormAccelerometerCalibrator(
20275                     gravityNorm, measurements, true,
20276                     ba, new Matrix(3, 1), this);
20277             fail("IllegalArgumentException expected but not thrown");
20278         } catch (final IllegalArgumentException ignore) {
20279         }
20280         assertNull(calibrator);
20281     }
20282 
20283     @Test
20284     public void testConstructor157() throws WrongSizeException {
20285         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20286         final double latitude = Math.toRadians(
20287                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20288         final double longitude = Math.toRadians(
20289                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20290         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20291         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20292         final NEDVelocity nedVelocity = new NEDVelocity();
20293         final ECEFPosition ecefPosition = new ECEFPosition();
20294         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20295         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20296                 ecefPosition, ecefVelocity);
20297         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20298                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20299         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20300 
20301         KnownGravityNormAccelerometerCalibrator calibrator =
20302                 new KnownGravityNormAccelerometerCalibrator(gravityNorm);
20303 
20304         // check default values
20305         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20306         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20307         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20308         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20309         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20310         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20311         final Acceleration bx2 = new Acceleration(0.0,
20312                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20313         calibrator.getInitialBiasXAsAcceleration(bx2);
20314         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20315         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20316         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20317         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20318         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20319         final Acceleration by2 = new Acceleration(0.0,
20320                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20321         calibrator.getInitialBiasYAsAcceleration(by2);
20322         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20323         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20324         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20325         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20326         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20327         final Acceleration bz2 = new Acceleration(0.0,
20328                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20329         calibrator.getInitialBiasZAsAcceleration(bz2);
20330         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20331         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20332         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20333         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20334         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20335         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20336         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20337         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20338         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20339         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20340         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20341         final double[] bias1 = calibrator.getInitialBias();
20342         assertArrayEquals(bias1, new double[3], 0.0);
20343         final double[] bias2 = new double[3];
20344         calibrator.getInitialBias(bias2);
20345         assertArrayEquals(bias1, bias2, 0.0);
20346         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20347         assertEquals(b1, new Matrix(3, 1));
20348         final Matrix b2 = new Matrix(3, 1);
20349         calibrator.getInitialBiasAsMatrix(b2);
20350         assertEquals(b1, b2);
20351         final Matrix ma1 = calibrator.getInitialMa();
20352         assertEquals(ma1, new Matrix(3, 3));
20353         final Matrix ma2 = new Matrix(3, 3);
20354         calibrator.getInitialMa(ma2);
20355         assertEquals(ma1, ma2);
20356         assertNull(calibrator.getMeasurements());
20357         assertFalse(calibrator.isCommonAxisUsed());
20358         assertNull(calibrator.getListener());
20359         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20360         assertFalse(calibrator.isReady());
20361         assertFalse(calibrator.isRunning());
20362         assertNull(calibrator.getEstimatedBiases());
20363         assertFalse(calibrator.getEstimatedBiases(null));
20364         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20365         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20366         assertNull(calibrator.getEstimatedBiasFx());
20367         assertNull(calibrator.getEstimatedBiasFy());
20368         assertNull(calibrator.getEstimatedBiasFz());
20369         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20370         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20371         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20372         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20373         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20374         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20375         assertNull(calibrator.getEstimatedMa());
20376         assertNull(calibrator.getEstimatedSx());
20377         assertNull(calibrator.getEstimatedSy());
20378         assertNull(calibrator.getEstimatedSz());
20379         assertNull(calibrator.getEstimatedMxy());
20380         assertNull(calibrator.getEstimatedMxz());
20381         assertNull(calibrator.getEstimatedMyx());
20382         assertNull(calibrator.getEstimatedMyz());
20383         assertNull(calibrator.getEstimatedMzx());
20384         assertNull(calibrator.getEstimatedMzy());
20385         assertNull(calibrator.getEstimatedCovariance());
20386         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20387         assertNotNull(calibrator.getGroundTruthGravityNorm());
20388         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20389         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20390         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20391                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20392         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20393         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20394         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20395 
20396         // Force IllegalArgumentException
20397         final Acceleration invalidGravityNorm = new Acceleration(
20398                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20399 
20400         calibrator = null;
20401         try {
20402             calibrator = new KnownGravityNormAccelerometerCalibrator(
20403                     invalidGravityNorm);
20404             fail("IllegalArgumentException expected but not thrown");
20405         } catch (final IllegalArgumentException ignore) {
20406         }
20407         assertNull(calibrator);
20408     }
20409 
20410     @Test
20411     public void testConstructor158() throws WrongSizeException {
20412         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20413         final double latitude = Math.toRadians(
20414                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20415         final double longitude = Math.toRadians(
20416                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20417         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20418         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20419         final NEDVelocity nedVelocity = new NEDVelocity();
20420         final ECEFPosition ecefPosition = new ECEFPosition();
20421         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20422         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20423                 ecefPosition, ecefVelocity);
20424         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20425                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20426         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20427 
20428         KnownGravityNormAccelerometerCalibrator calibrator =
20429                 new KnownGravityNormAccelerometerCalibrator(
20430                         gravityNorm, this);
20431 
20432         // check default values
20433         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20434         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20435         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20436         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20437         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20438         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20439         final Acceleration bx2 = new Acceleration(0.0,
20440                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20441         calibrator.getInitialBiasXAsAcceleration(bx2);
20442         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20443         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20444         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20445         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20446         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20447         final Acceleration by2 = new Acceleration(0.0,
20448                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20449         calibrator.getInitialBiasYAsAcceleration(by2);
20450         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20451         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20452         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20453         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20454         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20455         final Acceleration bz2 = new Acceleration(0.0,
20456                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20457         calibrator.getInitialBiasZAsAcceleration(bz2);
20458         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20459         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20460         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20461         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20462         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20463         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20464         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20465         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20466         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20467         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20468         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20469         final double[] bias1 = calibrator.getInitialBias();
20470         assertArrayEquals(bias1, new double[3], 0.0);
20471         final double[] bias2 = new double[3];
20472         calibrator.getInitialBias(bias2);
20473         assertArrayEquals(bias1, bias2, 0.0);
20474         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20475         assertEquals(b1, new Matrix(3, 1));
20476         final Matrix b2 = new Matrix(3, 1);
20477         calibrator.getInitialBiasAsMatrix(b2);
20478         assertEquals(b1, b2);
20479         final Matrix ma1 = calibrator.getInitialMa();
20480         assertEquals(ma1, new Matrix(3, 3));
20481         final Matrix ma2 = new Matrix(3, 3);
20482         calibrator.getInitialMa(ma2);
20483         assertEquals(ma1, ma2);
20484         assertNull(calibrator.getMeasurements());
20485         assertFalse(calibrator.isCommonAxisUsed());
20486         assertSame(calibrator.getListener(), this);
20487         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20488         assertFalse(calibrator.isReady());
20489         assertFalse(calibrator.isRunning());
20490         assertNull(calibrator.getEstimatedBiases());
20491         assertFalse(calibrator.getEstimatedBiases(null));
20492         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20493         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20494         assertNull(calibrator.getEstimatedBiasFx());
20495         assertNull(calibrator.getEstimatedBiasFy());
20496         assertNull(calibrator.getEstimatedBiasFz());
20497         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20498         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20499         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20500         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20501         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20502         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20503         assertNull(calibrator.getEstimatedMa());
20504         assertNull(calibrator.getEstimatedSx());
20505         assertNull(calibrator.getEstimatedSy());
20506         assertNull(calibrator.getEstimatedSz());
20507         assertNull(calibrator.getEstimatedMxy());
20508         assertNull(calibrator.getEstimatedMxz());
20509         assertNull(calibrator.getEstimatedMyx());
20510         assertNull(calibrator.getEstimatedMyz());
20511         assertNull(calibrator.getEstimatedMzx());
20512         assertNull(calibrator.getEstimatedMzy());
20513         assertNull(calibrator.getEstimatedCovariance());
20514         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20515         assertNotNull(calibrator.getGroundTruthGravityNorm());
20516         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20517         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20518         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20519                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20520         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20521         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20522         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20523 
20524         // Force IllegalArgumentException
20525         final Acceleration invalidGravityNorm = new Acceleration(
20526                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20527 
20528         calibrator = null;
20529         try {
20530             calibrator = new KnownGravityNormAccelerometerCalibrator(
20531                     invalidGravityNorm, this);
20532             fail("IllegalArgumentException expected but not thrown");
20533         } catch (final IllegalArgumentException ignore) {
20534         }
20535         assertNull(calibrator);
20536     }
20537 
20538     @Test
20539     public void testConstructor159() throws WrongSizeException {
20540         final Collection<StandardDeviationBodyKinematics> measurements =
20541                 Collections.emptyList();
20542 
20543         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20544         final double latitude = Math.toRadians(
20545                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20546         final double longitude = Math.toRadians(
20547                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20548         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20549         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20550         final NEDVelocity nedVelocity = new NEDVelocity();
20551         final ECEFPosition ecefPosition = new ECEFPosition();
20552         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20553         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20554                 ecefPosition, ecefVelocity);
20555         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20556                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20557         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20558 
20559         KnownGravityNormAccelerometerCalibrator calibrator =
20560                 new KnownGravityNormAccelerometerCalibrator(
20561                         gravityNorm, measurements);
20562 
20563         // check default values
20564         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20565         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20566         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20567         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20568         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20569         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20570         final Acceleration bx2 = new Acceleration(0.0,
20571                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20572         calibrator.getInitialBiasXAsAcceleration(bx2);
20573         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20574         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20575         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20576         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20577         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20578         final Acceleration by2 = new Acceleration(0.0,
20579                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20580         calibrator.getInitialBiasYAsAcceleration(by2);
20581         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20582         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20583         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20584         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20585         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20586         final Acceleration bz2 = new Acceleration(0.0,
20587                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20588         calibrator.getInitialBiasZAsAcceleration(bz2);
20589         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20590         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20591         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20592         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20593         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20594         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20595         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20596         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20597         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20598         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20599         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20600         final double[] bias1 = calibrator.getInitialBias();
20601         assertArrayEquals(bias1, new double[3], 0.0);
20602         final double[] bias2 = new double[3];
20603         calibrator.getInitialBias(bias2);
20604         assertArrayEquals(bias1, bias2, 0.0);
20605         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20606         assertEquals(b1, new Matrix(3, 1));
20607         final Matrix b2 = new Matrix(3, 1);
20608         calibrator.getInitialBiasAsMatrix(b2);
20609         assertEquals(b1, b2);
20610         final Matrix ma1 = calibrator.getInitialMa();
20611         assertEquals(ma1, new Matrix(3, 3));
20612         final Matrix ma2 = new Matrix(3, 3);
20613         calibrator.getInitialMa(ma2);
20614         assertEquals(ma1, ma2);
20615         assertSame(calibrator.getMeasurements(), measurements);
20616         assertFalse(calibrator.isCommonAxisUsed());
20617         assertNull(calibrator.getListener());
20618         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20619         assertFalse(calibrator.isReady());
20620         assertFalse(calibrator.isRunning());
20621         assertNull(calibrator.getEstimatedBiases());
20622         assertFalse(calibrator.getEstimatedBiases(null));
20623         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20624         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20625         assertNull(calibrator.getEstimatedBiasFx());
20626         assertNull(calibrator.getEstimatedBiasFy());
20627         assertNull(calibrator.getEstimatedBiasFz());
20628         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20629         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20630         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20631         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20632         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20633         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20634         assertNull(calibrator.getEstimatedMa());
20635         assertNull(calibrator.getEstimatedSx());
20636         assertNull(calibrator.getEstimatedSy());
20637         assertNull(calibrator.getEstimatedSz());
20638         assertNull(calibrator.getEstimatedMxy());
20639         assertNull(calibrator.getEstimatedMxz());
20640         assertNull(calibrator.getEstimatedMyx());
20641         assertNull(calibrator.getEstimatedMyz());
20642         assertNull(calibrator.getEstimatedMzx());
20643         assertNull(calibrator.getEstimatedMzy());
20644         assertNull(calibrator.getEstimatedCovariance());
20645         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20646         assertNotNull(calibrator.getGroundTruthGravityNorm());
20647         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20648         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20649         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20650                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20651         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20652         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20653         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20654 
20655         // Force IllegalArgumentException
20656         final Acceleration invalidGravityNorm = new Acceleration(
20657                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20658 
20659         calibrator = null;
20660         try {
20661             calibrator = new KnownGravityNormAccelerometerCalibrator(
20662                     invalidGravityNorm, measurements);
20663             fail("IllegalArgumentException expected but not thrown");
20664         } catch (final IllegalArgumentException ignore) {
20665         }
20666         assertNull(calibrator);
20667     }
20668 
20669     @Test
20670     public void testConstructor160() throws WrongSizeException {
20671         final Collection<StandardDeviationBodyKinematics> measurements =
20672                 Collections.emptyList();
20673 
20674         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20675         final double latitude = Math.toRadians(
20676                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20677         final double longitude = Math.toRadians(
20678                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20679         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20680         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20681         final NEDVelocity nedVelocity = new NEDVelocity();
20682         final ECEFPosition ecefPosition = new ECEFPosition();
20683         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20684         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20685                 ecefPosition, ecefVelocity);
20686         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20687                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20688         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20689 
20690         KnownGravityNormAccelerometerCalibrator calibrator =
20691                 new KnownGravityNormAccelerometerCalibrator(
20692                         gravityNorm, measurements, this);
20693 
20694         // check default values
20695         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20696         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20697         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20698         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20699         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20700         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20701         final Acceleration bx2 = new Acceleration(0.0,
20702                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20703         calibrator.getInitialBiasXAsAcceleration(bx2);
20704         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20705         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20706         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20707         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20708         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20709         final Acceleration by2 = new Acceleration(0.0,
20710                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20711         calibrator.getInitialBiasYAsAcceleration(by2);
20712         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20713         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20714         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20715         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20716         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20717         final Acceleration bz2 = new Acceleration(0.0,
20718                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20719         calibrator.getInitialBiasZAsAcceleration(bz2);
20720         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20721         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20722         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20723         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20724         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20725         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20726         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20727         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20728         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20729         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20730         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20731         final double[] bias1 = calibrator.getInitialBias();
20732         assertArrayEquals(bias1, new double[3], 0.0);
20733         final double[] bias2 = new double[3];
20734         calibrator.getInitialBias(bias2);
20735         assertArrayEquals(bias1, bias2, 0.0);
20736         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20737         assertEquals(b1, new Matrix(3, 1));
20738         final Matrix b2 = new Matrix(3, 1);
20739         calibrator.getInitialBiasAsMatrix(b2);
20740         assertEquals(b1, b2);
20741         final Matrix ma1 = calibrator.getInitialMa();
20742         assertEquals(ma1, new Matrix(3, 3));
20743         final Matrix ma2 = new Matrix(3, 3);
20744         calibrator.getInitialMa(ma2);
20745         assertEquals(ma1, ma2);
20746         assertSame(calibrator.getMeasurements(), measurements);
20747         assertFalse(calibrator.isCommonAxisUsed());
20748         assertSame(calibrator.getListener(), this);
20749         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20750         assertFalse(calibrator.isReady());
20751         assertFalse(calibrator.isRunning());
20752         assertNull(calibrator.getEstimatedBiases());
20753         assertFalse(calibrator.getEstimatedBiases(null));
20754         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20755         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20756         assertNull(calibrator.getEstimatedBiasFx());
20757         assertNull(calibrator.getEstimatedBiasFy());
20758         assertNull(calibrator.getEstimatedBiasFz());
20759         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20760         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20761         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20762         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20763         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20764         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20765         assertNull(calibrator.getEstimatedMa());
20766         assertNull(calibrator.getEstimatedSx());
20767         assertNull(calibrator.getEstimatedSy());
20768         assertNull(calibrator.getEstimatedSz());
20769         assertNull(calibrator.getEstimatedMxy());
20770         assertNull(calibrator.getEstimatedMxz());
20771         assertNull(calibrator.getEstimatedMyx());
20772         assertNull(calibrator.getEstimatedMyz());
20773         assertNull(calibrator.getEstimatedMzx());
20774         assertNull(calibrator.getEstimatedMzy());
20775         assertNull(calibrator.getEstimatedCovariance());
20776         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20777         assertNotNull(calibrator.getGroundTruthGravityNorm());
20778         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20779         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20780         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20781                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20782         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20783         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20784         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20785 
20786         // Force IllegalArgumentException
20787         final Acceleration invalidGravityNorm = new Acceleration(
20788                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20789 
20790         calibrator = null;
20791         try {
20792             calibrator = new KnownGravityNormAccelerometerCalibrator(
20793                     invalidGravityNorm, measurements, this);
20794             fail("IllegalArgumentException expected but not thrown");
20795         } catch (final IllegalArgumentException ignore) {
20796         }
20797         assertNull(calibrator);
20798     }
20799 
20800     @Test
20801     public void testConstructor161() throws WrongSizeException {
20802         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20803         final double latitude = Math.toRadians(
20804                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20805         final double longitude = Math.toRadians(
20806                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20807         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20808         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20809         final NEDVelocity nedVelocity = new NEDVelocity();
20810         final ECEFPosition ecefPosition = new ECEFPosition();
20811         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20812         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20813                 ecefPosition, ecefVelocity);
20814         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20815                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20816         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20817 
20818         KnownGravityNormAccelerometerCalibrator calibrator =
20819                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
20820                         true);
20821 
20822         // check default values
20823         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20824         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20825         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20826         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20827         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20828         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20829         final Acceleration bx2 = new Acceleration(0.0,
20830                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20831         calibrator.getInitialBiasXAsAcceleration(bx2);
20832         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20833         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20834         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20835         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20836         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20837         final Acceleration by2 = new Acceleration(0.0,
20838                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20839         calibrator.getInitialBiasYAsAcceleration(by2);
20840         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20841         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20842         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20843         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20844         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20845         final Acceleration bz2 = new Acceleration(0.0,
20846                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20847         calibrator.getInitialBiasZAsAcceleration(bz2);
20848         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20849         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20850         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20851         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20852         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20853         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20854         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20855         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20856         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20857         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20858         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20859         final double[] bias1 = calibrator.getInitialBias();
20860         assertArrayEquals(bias1, new double[3], 0.0);
20861         final double[] bias2 = new double[3];
20862         calibrator.getInitialBias(bias2);
20863         assertArrayEquals(bias1, bias2, 0.0);
20864         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20865         assertEquals(b1, new Matrix(3, 1));
20866         final Matrix b2 = new Matrix(3, 1);
20867         calibrator.getInitialBiasAsMatrix(b2);
20868         assertEquals(b1, b2);
20869         final Matrix ma1 = calibrator.getInitialMa();
20870         assertEquals(ma1, new Matrix(3, 3));
20871         final Matrix ma2 = new Matrix(3, 3);
20872         calibrator.getInitialMa(ma2);
20873         assertEquals(ma1, ma2);
20874         assertNull(calibrator.getMeasurements());
20875         assertTrue(calibrator.isCommonAxisUsed());
20876         assertNull(calibrator.getListener());
20877         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20878         assertFalse(calibrator.isReady());
20879         assertFalse(calibrator.isRunning());
20880         assertNull(calibrator.getEstimatedBiases());
20881         assertFalse(calibrator.getEstimatedBiases(null));
20882         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20883         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20884         assertNull(calibrator.getEstimatedBiasFx());
20885         assertNull(calibrator.getEstimatedBiasFy());
20886         assertNull(calibrator.getEstimatedBiasFz());
20887         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20888         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20889         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20890         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20891         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20892         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20893         assertNull(calibrator.getEstimatedMa());
20894         assertNull(calibrator.getEstimatedSx());
20895         assertNull(calibrator.getEstimatedSy());
20896         assertNull(calibrator.getEstimatedSz());
20897         assertNull(calibrator.getEstimatedMxy());
20898         assertNull(calibrator.getEstimatedMxz());
20899         assertNull(calibrator.getEstimatedMyx());
20900         assertNull(calibrator.getEstimatedMyz());
20901         assertNull(calibrator.getEstimatedMzx());
20902         assertNull(calibrator.getEstimatedMzy());
20903         assertNull(calibrator.getEstimatedCovariance());
20904         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20905         assertNotNull(calibrator.getGroundTruthGravityNorm());
20906         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20907         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20908         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20909                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20910         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20911         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20912         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20913 
20914         // Force IllegalArgumentException
20915         final Acceleration invalidGravityNorm = new Acceleration(
20916                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20917 
20918         calibrator = null;
20919         try {
20920             calibrator = new KnownGravityNormAccelerometerCalibrator(
20921                     invalidGravityNorm, true);
20922             fail("IllegalArgumentException expected but not thrown");
20923         } catch (final IllegalArgumentException ignore) {
20924         }
20925         assertNull(calibrator);
20926     }
20927 
20928     @Test
20929     public void testConstructor162() throws WrongSizeException {
20930         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20931         final double latitude = Math.toRadians(
20932                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20933         final double longitude = Math.toRadians(
20934                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20935         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20936         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20937         final NEDVelocity nedVelocity = new NEDVelocity();
20938         final ECEFPosition ecefPosition = new ECEFPosition();
20939         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20940         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20941                 ecefPosition, ecefVelocity);
20942         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20943                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20944         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
20945 
20946         KnownGravityNormAccelerometerCalibrator calibrator =
20947                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
20948                         true, this);
20949 
20950         // check default values
20951         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20952         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20953         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20954         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20955         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20956         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20957         final Acceleration bx2 = new Acceleration(0.0,
20958                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20959         calibrator.getInitialBiasXAsAcceleration(bx2);
20960         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20961         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20962         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20963         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20964         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20965         final Acceleration by2 = new Acceleration(0.0,
20966                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20967         calibrator.getInitialBiasYAsAcceleration(by2);
20968         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20969         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20970         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20971         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20972         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20973         final Acceleration bz2 = new Acceleration(0.0,
20974                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20975         calibrator.getInitialBiasZAsAcceleration(bz2);
20976         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20977         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20978         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20979         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20980         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20981         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20982         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20983         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20984         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20985         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20986         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20987         final double[] bias1 = calibrator.getInitialBias();
20988         assertArrayEquals(bias1, new double[3], 0.0);
20989         final double[] bias2 = new double[3];
20990         calibrator.getInitialBias(bias2);
20991         assertArrayEquals(bias1, bias2, 0.0);
20992         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20993         assertEquals(b1, new Matrix(3, 1));
20994         final Matrix b2 = new Matrix(3, 1);
20995         calibrator.getInitialBiasAsMatrix(b2);
20996         assertEquals(b1, b2);
20997         final Matrix ma1 = calibrator.getInitialMa();
20998         assertEquals(ma1, new Matrix(3, 3));
20999         final Matrix ma2 = new Matrix(3, 3);
21000         calibrator.getInitialMa(ma2);
21001         assertEquals(ma1, ma2);
21002         assertNull(calibrator.getMeasurements());
21003         assertTrue(calibrator.isCommonAxisUsed());
21004         assertSame(calibrator.getListener(), this);
21005         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21006         assertFalse(calibrator.isReady());
21007         assertFalse(calibrator.isRunning());
21008         assertNull(calibrator.getEstimatedBiases());
21009         assertFalse(calibrator.getEstimatedBiases(null));
21010         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21011         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21012         assertNull(calibrator.getEstimatedBiasFx());
21013         assertNull(calibrator.getEstimatedBiasFy());
21014         assertNull(calibrator.getEstimatedBiasFz());
21015         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21016         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21017         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21018         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21019         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21020         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21021         assertNull(calibrator.getEstimatedMa());
21022         assertNull(calibrator.getEstimatedSx());
21023         assertNull(calibrator.getEstimatedSy());
21024         assertNull(calibrator.getEstimatedSz());
21025         assertNull(calibrator.getEstimatedMxy());
21026         assertNull(calibrator.getEstimatedMxz());
21027         assertNull(calibrator.getEstimatedMyx());
21028         assertNull(calibrator.getEstimatedMyz());
21029         assertNull(calibrator.getEstimatedMzx());
21030         assertNull(calibrator.getEstimatedMzy());
21031         assertNull(calibrator.getEstimatedCovariance());
21032         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21033         assertNotNull(calibrator.getGroundTruthGravityNorm());
21034         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21035         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21036         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21037                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21038         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21039         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21040         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21041 
21042         // Force IllegalArgumentException
21043         final Acceleration invalidGravityNorm = new Acceleration(
21044                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21045 
21046         calibrator = null;
21047         try {
21048             calibrator = new KnownGravityNormAccelerometerCalibrator(
21049                     invalidGravityNorm, true, this);
21050             fail("IllegalArgumentException expected but not thrown");
21051         } catch (final IllegalArgumentException ignore) {
21052         }
21053         assertNull(calibrator);
21054     }
21055 
21056     @Test
21057     public void testConstructor163() throws WrongSizeException {
21058         final Collection<StandardDeviationBodyKinematics> measurements =
21059                 Collections.emptyList();
21060         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21061         final double latitude = Math.toRadians(
21062                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21063         final double longitude = Math.toRadians(
21064                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21065         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21066         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21067         final NEDVelocity nedVelocity = new NEDVelocity();
21068         final ECEFPosition ecefPosition = new ECEFPosition();
21069         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21070         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21071                 ecefPosition, ecefVelocity);
21072         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21073                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21074         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21075 
21076         KnownGravityNormAccelerometerCalibrator calibrator =
21077                 new KnownGravityNormAccelerometerCalibrator(
21078                         gravityNorm, measurements,
21079                         true);
21080 
21081         // check default values
21082         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
21083         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
21084         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
21085         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21086         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
21087         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21088         final Acceleration bx2 = new Acceleration(0.0,
21089                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21090         calibrator.getInitialBiasXAsAcceleration(bx2);
21091         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
21092         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21093         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21094         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
21095         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21096         final Acceleration by2 = new Acceleration(0.0,
21097                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21098         calibrator.getInitialBiasYAsAcceleration(by2);
21099         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
21100         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21101         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21102         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
21103         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21104         final Acceleration bz2 = new Acceleration(0.0,
21105                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21106         calibrator.getInitialBiasZAsAcceleration(bz2);
21107         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
21108         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21109         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21110         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21111         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21112         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21113         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21114         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21115         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21116         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21117         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21118         final double[] bias1 = calibrator.getInitialBias();
21119         assertArrayEquals(bias1, new double[3], 0.0);
21120         final double[] bias2 = new double[3];
21121         calibrator.getInitialBias(bias2);
21122         assertArrayEquals(bias1, bias2, 0.0);
21123         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21124         assertEquals(b1, new Matrix(3, 1));
21125         final Matrix b2 = new Matrix(3, 1);
21126         calibrator.getInitialBiasAsMatrix(b2);
21127         assertEquals(b1, b2);
21128         final Matrix ma1 = calibrator.getInitialMa();
21129         assertEquals(ma1, new Matrix(3, 3));
21130         final Matrix ma2 = new Matrix(3, 3);
21131         calibrator.getInitialMa(ma2);
21132         assertEquals(ma1, ma2);
21133         assertSame(calibrator.getMeasurements(), measurements);
21134         assertTrue(calibrator.isCommonAxisUsed());
21135         assertNull(calibrator.getListener());
21136         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21137         assertFalse(calibrator.isReady());
21138         assertFalse(calibrator.isRunning());
21139         assertNull(calibrator.getEstimatedBiases());
21140         assertFalse(calibrator.getEstimatedBiases(null));
21141         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21142         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21143         assertNull(calibrator.getEstimatedBiasFx());
21144         assertNull(calibrator.getEstimatedBiasFy());
21145         assertNull(calibrator.getEstimatedBiasFz());
21146         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21147         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21148         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21149         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21150         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21151         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21152         assertNull(calibrator.getEstimatedMa());
21153         assertNull(calibrator.getEstimatedSx());
21154         assertNull(calibrator.getEstimatedSy());
21155         assertNull(calibrator.getEstimatedSz());
21156         assertNull(calibrator.getEstimatedMxy());
21157         assertNull(calibrator.getEstimatedMxz());
21158         assertNull(calibrator.getEstimatedMyx());
21159         assertNull(calibrator.getEstimatedMyz());
21160         assertNull(calibrator.getEstimatedMzx());
21161         assertNull(calibrator.getEstimatedMzy());
21162         assertNull(calibrator.getEstimatedCovariance());
21163         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21164         assertNotNull(calibrator.getGroundTruthGravityNorm());
21165         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21166         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21167         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21168                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21169         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21170         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21171         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21172 
21173         // Force IllegalArgumentException
21174         final Acceleration invalidGravityNorm = new Acceleration(
21175                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21176 
21177         calibrator = null;
21178         try {
21179             calibrator = new KnownGravityNormAccelerometerCalibrator(
21180                     invalidGravityNorm, measurements,
21181                     true);
21182             fail("IllegalArgumentException expected but not thrown");
21183         } catch (final IllegalArgumentException ignore) {
21184         }
21185         assertNull(calibrator);
21186     }
21187 
21188     @Test
21189     public void testConstructor164() throws WrongSizeException {
21190         final Collection<StandardDeviationBodyKinematics> measurements =
21191                 Collections.emptyList();
21192         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21193         final double latitude = Math.toRadians(
21194                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21195         final double longitude = Math.toRadians(
21196                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21197         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21198         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21199         final NEDVelocity nedVelocity = new NEDVelocity();
21200         final ECEFPosition ecefPosition = new ECEFPosition();
21201         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21202         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21203                 ecefPosition, ecefVelocity);
21204         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21205                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21206         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21207 
21208         KnownGravityNormAccelerometerCalibrator calibrator =
21209                 new KnownGravityNormAccelerometerCalibrator(
21210                         gravityNorm, measurements,
21211                         true, this);
21212 
21213         // check default values
21214         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
21215         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
21216         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
21217         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21218         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
21219         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21220         final Acceleration bx2 = new Acceleration(0.0,
21221                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21222         calibrator.getInitialBiasXAsAcceleration(bx2);
21223         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
21224         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21225         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21226         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
21227         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21228         final Acceleration by2 = new Acceleration(0.0,
21229                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21230         calibrator.getInitialBiasYAsAcceleration(by2);
21231         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
21232         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21233         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21234         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
21235         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21236         final Acceleration bz2 = new Acceleration(0.0,
21237                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21238         calibrator.getInitialBiasZAsAcceleration(bz2);
21239         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
21240         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21241         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21242         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21243         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21244         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21245         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21246         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21247         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21248         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21249         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21250         final double[] bias1 = calibrator.getInitialBias();
21251         assertArrayEquals(bias1, new double[3], 0.0);
21252         final double[] bias2 = new double[3];
21253         calibrator.getInitialBias(bias2);
21254         assertArrayEquals(bias1, bias2, 0.0);
21255         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21256         assertEquals(b1, new Matrix(3, 1));
21257         final Matrix b2 = new Matrix(3, 1);
21258         calibrator.getInitialBiasAsMatrix(b2);
21259         assertEquals(b1, b2);
21260         final Matrix ma1 = calibrator.getInitialMa();
21261         assertEquals(ma1, new Matrix(3, 3));
21262         final Matrix ma2 = new Matrix(3, 3);
21263         calibrator.getInitialMa(ma2);
21264         assertEquals(ma1, ma2);
21265         assertSame(calibrator.getMeasurements(), measurements);
21266         assertTrue(calibrator.isCommonAxisUsed());
21267         assertSame(calibrator.getListener(), this);
21268         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21269         assertFalse(calibrator.isReady());
21270         assertFalse(calibrator.isRunning());
21271         assertNull(calibrator.getEstimatedBiases());
21272         assertFalse(calibrator.getEstimatedBiases(null));
21273         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21274         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21275         assertNull(calibrator.getEstimatedBiasFx());
21276         assertNull(calibrator.getEstimatedBiasFy());
21277         assertNull(calibrator.getEstimatedBiasFz());
21278         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21279         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21280         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21281         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21282         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21283         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21284         assertNull(calibrator.getEstimatedMa());
21285         assertNull(calibrator.getEstimatedSx());
21286         assertNull(calibrator.getEstimatedSy());
21287         assertNull(calibrator.getEstimatedSz());
21288         assertNull(calibrator.getEstimatedMxy());
21289         assertNull(calibrator.getEstimatedMxz());
21290         assertNull(calibrator.getEstimatedMyx());
21291         assertNull(calibrator.getEstimatedMyz());
21292         assertNull(calibrator.getEstimatedMzx());
21293         assertNull(calibrator.getEstimatedMzy());
21294         assertNull(calibrator.getEstimatedCovariance());
21295         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21296         assertNotNull(calibrator.getGroundTruthGravityNorm());
21297         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21298         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21299         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21300                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21301         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21302         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21303         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21304 
21305         // Force IllegalArgumentException
21306         final Acceleration invalidGravityNorm = new Acceleration(
21307                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21308 
21309         calibrator = null;
21310         try {
21311             calibrator = new KnownGravityNormAccelerometerCalibrator(
21312                     invalidGravityNorm, measurements,
21313                     true, this);
21314             fail("IllegalArgumentException expected but not thrown");
21315         } catch (final IllegalArgumentException ignore) {
21316         }
21317         assertNull(calibrator);
21318     }
21319 
21320     @Test
21321     public void testConstructor165() throws WrongSizeException {
21322         final Matrix ba = generateBa();
21323         final double biasX = ba.getElementAtIndex(0);
21324         final double biasY = ba.getElementAtIndex(1);
21325         final double biasZ = ba.getElementAtIndex(2);
21326 
21327         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21328         final double latitude = Math.toRadians(
21329                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21330         final double longitude = Math.toRadians(
21331                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21332         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21333         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21334         final NEDVelocity nedVelocity = new NEDVelocity();
21335         final ECEFPosition ecefPosition = new ECEFPosition();
21336         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21337         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21338                 ecefPosition, ecefVelocity);
21339         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21340                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21341         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21342 
21343         KnownGravityNormAccelerometerCalibrator calibrator =
21344                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
21345                         biasX, biasY, biasZ);
21346 
21347         // check default values
21348         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21349         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21350         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21351         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21352         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21353         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21354         final Acceleration bx2 = new Acceleration(0.0,
21355                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21356         calibrator.getInitialBiasXAsAcceleration(bx2);
21357         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21358         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21359         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21360         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21361         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21362         final Acceleration by2 = new Acceleration(0.0,
21363                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21364         calibrator.getInitialBiasYAsAcceleration(by2);
21365         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21366         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21367         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21368         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21369         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21370         final Acceleration bz2 = new Acceleration(0.0,
21371                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21372         calibrator.getInitialBiasZAsAcceleration(bz2);
21373         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21374         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21375         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21376         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21377         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21378         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21379         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21380         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21381         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21382         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21383         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21384         final double[] bias1 = calibrator.getInitialBias();
21385         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21386         final double[] bias2 = new double[3];
21387         calibrator.getInitialBias(bias2);
21388         assertArrayEquals(bias1, bias2, 0.0);
21389         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21390         assertEquals(b1, ba);
21391         final Matrix b2 = new Matrix(3, 1);
21392         calibrator.getInitialBiasAsMatrix(b2);
21393         assertEquals(b1, b2);
21394         final Matrix ma1 = calibrator.getInitialMa();
21395         assertEquals(ma1, new Matrix(3, 3));
21396         final Matrix ma2 = new Matrix(3, 3);
21397         calibrator.getInitialMa(ma2);
21398         assertEquals(ma1, ma2);
21399         assertNull(calibrator.getMeasurements());
21400         assertFalse(calibrator.isCommonAxisUsed());
21401         assertNull(calibrator.getListener());
21402         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21403         assertFalse(calibrator.isReady());
21404         assertFalse(calibrator.isRunning());
21405         assertNull(calibrator.getEstimatedBiases());
21406         assertFalse(calibrator.getEstimatedBiases(null));
21407         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21408         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21409         assertNull(calibrator.getEstimatedBiasFx());
21410         assertNull(calibrator.getEstimatedBiasFy());
21411         assertNull(calibrator.getEstimatedBiasFz());
21412         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21413         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21414         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21415         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21416         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21417         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21418         assertNull(calibrator.getEstimatedMa());
21419         assertNull(calibrator.getEstimatedSx());
21420         assertNull(calibrator.getEstimatedSy());
21421         assertNull(calibrator.getEstimatedSz());
21422         assertNull(calibrator.getEstimatedMxy());
21423         assertNull(calibrator.getEstimatedMxz());
21424         assertNull(calibrator.getEstimatedMyx());
21425         assertNull(calibrator.getEstimatedMyz());
21426         assertNull(calibrator.getEstimatedMzx());
21427         assertNull(calibrator.getEstimatedMzy());
21428         assertNull(calibrator.getEstimatedCovariance());
21429         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21430         assertNotNull(calibrator.getGroundTruthGravityNorm());
21431         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21432         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21433         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21434                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21435         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21436         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21437         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21438 
21439         // Force IllegalArgumentException
21440         final Acceleration invalidGravityNorm = new Acceleration(
21441                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21442 
21443         calibrator = null;
21444         try {
21445             calibrator = new KnownGravityNormAccelerometerCalibrator(
21446                     invalidGravityNorm, biasX, biasY, biasZ);
21447             fail("IllegalArgumentException expected but not thrown");
21448         } catch (final IllegalArgumentException ignore) {
21449         }
21450         assertNull(calibrator);
21451     }
21452 
21453     @Test
21454     public void testConstructor166() throws WrongSizeException {
21455         final Matrix ba = generateBa();
21456         final double biasX = ba.getElementAtIndex(0);
21457         final double biasY = ba.getElementAtIndex(1);
21458         final double biasZ = ba.getElementAtIndex(2);
21459 
21460         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21461         final double latitude = Math.toRadians(
21462                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21463         final double longitude = Math.toRadians(
21464                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21465         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21466         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21467         final NEDVelocity nedVelocity = new NEDVelocity();
21468         final ECEFPosition ecefPosition = new ECEFPosition();
21469         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21470         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21471                 ecefPosition, ecefVelocity);
21472         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21473                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21474         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21475 
21476         KnownGravityNormAccelerometerCalibrator calibrator =
21477                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
21478                         biasX, biasY, biasZ, this);
21479 
21480         // check default values
21481         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21482         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21483         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21484         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21485         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21486         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21487         final Acceleration bx2 = new Acceleration(0.0,
21488                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21489         calibrator.getInitialBiasXAsAcceleration(bx2);
21490         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21491         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21492         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21493         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21494         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21495         final Acceleration by2 = new Acceleration(0.0,
21496                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21497         calibrator.getInitialBiasYAsAcceleration(by2);
21498         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21499         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21500         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21501         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21502         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21503         final Acceleration bz2 = new Acceleration(0.0,
21504                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21505         calibrator.getInitialBiasZAsAcceleration(bz2);
21506         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21507         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21508         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21509         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21510         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21511         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21512         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21513         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21514         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21515         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21516         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21517         final double[] bias1 = calibrator.getInitialBias();
21518         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21519         final double[] bias2 = new double[3];
21520         calibrator.getInitialBias(bias2);
21521         assertArrayEquals(bias1, bias2, 0.0);
21522         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21523         assertEquals(b1, ba);
21524         final Matrix b2 = new Matrix(3, 1);
21525         calibrator.getInitialBiasAsMatrix(b2);
21526         assertEquals(b1, b2);
21527         final Matrix ma1 = calibrator.getInitialMa();
21528         assertEquals(ma1, new Matrix(3, 3));
21529         final Matrix ma2 = new Matrix(3, 3);
21530         calibrator.getInitialMa(ma2);
21531         assertEquals(ma1, ma2);
21532         assertNull(calibrator.getMeasurements());
21533         assertFalse(calibrator.isCommonAxisUsed());
21534         assertSame(calibrator.getListener(), this);
21535         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21536         assertFalse(calibrator.isReady());
21537         assertFalse(calibrator.isRunning());
21538         assertNull(calibrator.getEstimatedBiases());
21539         assertFalse(calibrator.getEstimatedBiases(null));
21540         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21541         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21542         assertNull(calibrator.getEstimatedBiasFx());
21543         assertNull(calibrator.getEstimatedBiasFy());
21544         assertNull(calibrator.getEstimatedBiasFz());
21545         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21546         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21547         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21548         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21549         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21550         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21551         assertNull(calibrator.getEstimatedMa());
21552         assertNull(calibrator.getEstimatedSx());
21553         assertNull(calibrator.getEstimatedSy());
21554         assertNull(calibrator.getEstimatedSz());
21555         assertNull(calibrator.getEstimatedMxy());
21556         assertNull(calibrator.getEstimatedMxz());
21557         assertNull(calibrator.getEstimatedMyx());
21558         assertNull(calibrator.getEstimatedMyz());
21559         assertNull(calibrator.getEstimatedMzx());
21560         assertNull(calibrator.getEstimatedMzy());
21561         assertNull(calibrator.getEstimatedCovariance());
21562         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21563         assertNotNull(calibrator.getGroundTruthGravityNorm());
21564         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21565         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21566         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21567                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21568         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21569         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21570         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21571 
21572         // Force IllegalArgumentException
21573         final Acceleration invalidGravityNorm = new Acceleration(
21574                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21575 
21576         calibrator = null;
21577         try {
21578             calibrator = new KnownGravityNormAccelerometerCalibrator(
21579                     invalidGravityNorm, biasX, biasY, biasZ,
21580                     this);
21581             fail("IllegalArgumentException expected but not thrown");
21582         } catch (final IllegalArgumentException ignore) {
21583         }
21584         assertNull(calibrator);
21585     }
21586 
21587     @Test
21588     public void testConstructor167() throws WrongSizeException {
21589         final Collection<StandardDeviationBodyKinematics> measurements =
21590                 Collections.emptyList();
21591 
21592         final Matrix ba = generateBa();
21593         final double biasX = ba.getElementAtIndex(0);
21594         final double biasY = ba.getElementAtIndex(1);
21595         final double biasZ = ba.getElementAtIndex(2);
21596 
21597         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21598         final double latitude = Math.toRadians(
21599                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21600         final double longitude = Math.toRadians(
21601                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21602         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21603         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21604         final NEDVelocity nedVelocity = new NEDVelocity();
21605         final ECEFPosition ecefPosition = new ECEFPosition();
21606         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21607         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21608                 ecefPosition, ecefVelocity);
21609         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21610                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21611         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21612 
21613         KnownGravityNormAccelerometerCalibrator calibrator =
21614                 new KnownGravityNormAccelerometerCalibrator(
21615                         gravityNorm, measurements,
21616                         biasX, biasY, biasZ);
21617 
21618         // check default values
21619         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21620         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21621         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21622         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21623         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21624         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21625         final Acceleration bx2 = new Acceleration(0.0,
21626                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21627         calibrator.getInitialBiasXAsAcceleration(bx2);
21628         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21629         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21630         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21631         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21632         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21633         final Acceleration by2 = new Acceleration(0.0,
21634                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21635         calibrator.getInitialBiasYAsAcceleration(by2);
21636         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21637         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21638         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21639         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21640         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21641         final Acceleration bz2 = new Acceleration(0.0,
21642                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21643         calibrator.getInitialBiasZAsAcceleration(bz2);
21644         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21645         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21646         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21647         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21648         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21649         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21650         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21651         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21652         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21653         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21654         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21655         final double[] bias1 = calibrator.getInitialBias();
21656         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21657         final double[] bias2 = new double[3];
21658         calibrator.getInitialBias(bias2);
21659         assertArrayEquals(bias1, bias2, 0.0);
21660         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21661         assertEquals(b1, ba);
21662         final Matrix b2 = new Matrix(3, 1);
21663         calibrator.getInitialBiasAsMatrix(b2);
21664         assertEquals(b1, b2);
21665         final Matrix ma1 = calibrator.getInitialMa();
21666         assertEquals(ma1, new Matrix(3, 3));
21667         final Matrix ma2 = new Matrix(3, 3);
21668         calibrator.getInitialMa(ma2);
21669         assertEquals(ma1, ma2);
21670         assertSame(calibrator.getMeasurements(), measurements);
21671         assertFalse(calibrator.isCommonAxisUsed());
21672         assertNull(calibrator.getListener());
21673         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21674         assertFalse(calibrator.isReady());
21675         assertFalse(calibrator.isRunning());
21676         assertNull(calibrator.getEstimatedBiases());
21677         assertFalse(calibrator.getEstimatedBiases(null));
21678         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21679         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21680         assertNull(calibrator.getEstimatedBiasFx());
21681         assertNull(calibrator.getEstimatedBiasFy());
21682         assertNull(calibrator.getEstimatedBiasFz());
21683         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21684         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21685         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21686         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21687         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21688         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21689         assertNull(calibrator.getEstimatedMa());
21690         assertNull(calibrator.getEstimatedSx());
21691         assertNull(calibrator.getEstimatedSy());
21692         assertNull(calibrator.getEstimatedSz());
21693         assertNull(calibrator.getEstimatedMxy());
21694         assertNull(calibrator.getEstimatedMxz());
21695         assertNull(calibrator.getEstimatedMyx());
21696         assertNull(calibrator.getEstimatedMyz());
21697         assertNull(calibrator.getEstimatedMzx());
21698         assertNull(calibrator.getEstimatedMzy());
21699         assertNull(calibrator.getEstimatedCovariance());
21700         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21701         assertNotNull(calibrator.getGroundTruthGravityNorm());
21702         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21703         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21704         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21705                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21706         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21707         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21708         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21709 
21710         // Force IllegalArgumentException
21711         final Acceleration invalidGravityNorm = new Acceleration(
21712                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21713 
21714         calibrator = null;
21715         try {
21716             calibrator = new KnownGravityNormAccelerometerCalibrator(
21717                     invalidGravityNorm, measurements,
21718                     biasX, biasY, biasZ);
21719             fail("IllegalArgumentException expected but not thrown");
21720         } catch (final IllegalArgumentException ignore) {
21721         }
21722         assertNull(calibrator);
21723     }
21724 
21725     @Test
21726     public void testConstructor168() throws WrongSizeException {
21727         final Collection<StandardDeviationBodyKinematics> measurements =
21728                 Collections.emptyList();
21729 
21730         final Matrix ba = generateBa();
21731         final double biasX = ba.getElementAtIndex(0);
21732         final double biasY = ba.getElementAtIndex(1);
21733         final double biasZ = ba.getElementAtIndex(2);
21734 
21735         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21736         final double latitude = Math.toRadians(
21737                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21738         final double longitude = Math.toRadians(
21739                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21740         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21741         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21742         final NEDVelocity nedVelocity = new NEDVelocity();
21743         final ECEFPosition ecefPosition = new ECEFPosition();
21744         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21745         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21746                 ecefPosition, ecefVelocity);
21747         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21748                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21749         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21750 
21751         KnownGravityNormAccelerometerCalibrator calibrator =
21752                 new KnownGravityNormAccelerometerCalibrator(
21753                         gravityNorm, measurements,
21754                         biasX, biasY, biasZ, this);
21755 
21756         // check default values
21757         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21758         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21759         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21760         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21761         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21762         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21763         final Acceleration bx2 = new Acceleration(0.0,
21764                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21765         calibrator.getInitialBiasXAsAcceleration(bx2);
21766         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21767         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21768         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21769         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21770         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21771         final Acceleration by2 = new Acceleration(0.0,
21772                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21773         calibrator.getInitialBiasYAsAcceleration(by2);
21774         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21775         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21776         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21777         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21778         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21779         final Acceleration bz2 = new Acceleration(0.0,
21780                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21781         calibrator.getInitialBiasZAsAcceleration(bz2);
21782         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21783         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21784         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21785         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21786         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21787         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21788         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21789         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21790         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21791         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21792         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21793         final double[] bias1 = calibrator.getInitialBias();
21794         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21795         final double[] bias2 = new double[3];
21796         calibrator.getInitialBias(bias2);
21797         assertArrayEquals(bias1, bias2, 0.0);
21798         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21799         assertEquals(b1, ba);
21800         final Matrix b2 = new Matrix(3, 1);
21801         calibrator.getInitialBiasAsMatrix(b2);
21802         assertEquals(b1, b2);
21803         final Matrix ma1 = calibrator.getInitialMa();
21804         assertEquals(ma1, new Matrix(3, 3));
21805         final Matrix ma2 = new Matrix(3, 3);
21806         calibrator.getInitialMa(ma2);
21807         assertEquals(ma1, ma2);
21808         assertSame(calibrator.getMeasurements(), measurements);
21809         assertFalse(calibrator.isCommonAxisUsed());
21810         assertSame(calibrator.getListener(), this);
21811         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21812         assertFalse(calibrator.isReady());
21813         assertFalse(calibrator.isRunning());
21814         assertNull(calibrator.getEstimatedBiases());
21815         assertFalse(calibrator.getEstimatedBiases(null));
21816         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21817         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21818         assertNull(calibrator.getEstimatedBiasFx());
21819         assertNull(calibrator.getEstimatedBiasFy());
21820         assertNull(calibrator.getEstimatedBiasFz());
21821         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21822         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21823         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21824         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21825         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21826         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21827         assertNull(calibrator.getEstimatedMa());
21828         assertNull(calibrator.getEstimatedSx());
21829         assertNull(calibrator.getEstimatedSy());
21830         assertNull(calibrator.getEstimatedSz());
21831         assertNull(calibrator.getEstimatedMxy());
21832         assertNull(calibrator.getEstimatedMxz());
21833         assertNull(calibrator.getEstimatedMyx());
21834         assertNull(calibrator.getEstimatedMyz());
21835         assertNull(calibrator.getEstimatedMzx());
21836         assertNull(calibrator.getEstimatedMzy());
21837         assertNull(calibrator.getEstimatedCovariance());
21838         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21839         assertNotNull(calibrator.getGroundTruthGravityNorm());
21840         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21841         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21842         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21843                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21844         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21845         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21846         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21847 
21848         // Force IllegalArgumentException
21849         final Acceleration invalidGravityNorm = new Acceleration(
21850                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21851 
21852         calibrator = null;
21853         try {
21854             calibrator = new KnownGravityNormAccelerometerCalibrator(
21855                     invalidGravityNorm, measurements,
21856                     biasX, biasY, biasZ, this);
21857             fail("IllegalArgumentException expected but not thrown");
21858         } catch (final IllegalArgumentException ignore) {
21859         }
21860         assertNull(calibrator);
21861     }
21862 
21863     @Test
21864     public void testConstructor169() throws WrongSizeException {
21865         final Matrix ba = generateBa();
21866         final double biasX = ba.getElementAtIndex(0);
21867         final double biasY = ba.getElementAtIndex(1);
21868         final double biasZ = ba.getElementAtIndex(2);
21869 
21870         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21871         final double latitude = Math.toRadians(
21872                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21873         final double longitude = Math.toRadians(
21874                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21875         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21876         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21877         final NEDVelocity nedVelocity = new NEDVelocity();
21878         final ECEFPosition ecefPosition = new ECEFPosition();
21879         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21880         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21881                 ecefPosition, ecefVelocity);
21882         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21883                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21884         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
21885 
21886         KnownGravityNormAccelerometerCalibrator calibrator =
21887                 new KnownGravityNormAccelerometerCalibrator(
21888                         gravityNorm, true, biasX, biasY, biasZ);
21889 
21890         // check default values
21891         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21892         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21893         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21894         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21895         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21896         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21897         final Acceleration bx2 = new Acceleration(0.0,
21898                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21899         calibrator.getInitialBiasXAsAcceleration(bx2);
21900         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21901         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21902         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21903         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21904         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21905         final Acceleration by2 = new Acceleration(0.0,
21906                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21907         calibrator.getInitialBiasYAsAcceleration(by2);
21908         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21909         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21910         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21911         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21912         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21913         final Acceleration bz2 = new Acceleration(0.0,
21914                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21915         calibrator.getInitialBiasZAsAcceleration(bz2);
21916         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21917         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21918         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21919         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21920         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21921         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21922         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21923         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21924         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21925         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21926         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21927         final double[] bias1 = calibrator.getInitialBias();
21928         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21929         final double[] bias2 = new double[3];
21930         calibrator.getInitialBias(bias2);
21931         assertArrayEquals(bias1, bias2, 0.0);
21932         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21933         assertEquals(b1, ba);
21934         final Matrix b2 = new Matrix(3, 1);
21935         calibrator.getInitialBiasAsMatrix(b2);
21936         assertEquals(b1, b2);
21937         final Matrix ma1 = calibrator.getInitialMa();
21938         assertEquals(ma1, new Matrix(3, 3));
21939         final Matrix ma2 = new Matrix(3, 3);
21940         calibrator.getInitialMa(ma2);
21941         assertEquals(ma1, ma2);
21942         assertNull(calibrator.getMeasurements());
21943         assertTrue(calibrator.isCommonAxisUsed());
21944         assertNull(calibrator.getListener());
21945         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21946         assertFalse(calibrator.isReady());
21947         assertFalse(calibrator.isRunning());
21948         assertNull(calibrator.getEstimatedBiases());
21949         assertFalse(calibrator.getEstimatedBiases(null));
21950         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21951         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21952         assertNull(calibrator.getEstimatedBiasFx());
21953         assertNull(calibrator.getEstimatedBiasFy());
21954         assertNull(calibrator.getEstimatedBiasFz());
21955         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21956         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21957         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21958         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21959         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21960         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21961         assertNull(calibrator.getEstimatedMa());
21962         assertNull(calibrator.getEstimatedSx());
21963         assertNull(calibrator.getEstimatedSy());
21964         assertNull(calibrator.getEstimatedSz());
21965         assertNull(calibrator.getEstimatedMxy());
21966         assertNull(calibrator.getEstimatedMxz());
21967         assertNull(calibrator.getEstimatedMyx());
21968         assertNull(calibrator.getEstimatedMyz());
21969         assertNull(calibrator.getEstimatedMzx());
21970         assertNull(calibrator.getEstimatedMzy());
21971         assertNull(calibrator.getEstimatedCovariance());
21972         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21973         assertNotNull(calibrator.getGroundTruthGravityNorm());
21974         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21975         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21976         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21977                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21978         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21979         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21980         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21981 
21982         // Force IllegalArgumentException
21983         final Acceleration invalidGravityNorm = new Acceleration(
21984                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21985 
21986         calibrator = null;
21987         try {
21988             calibrator = new KnownGravityNormAccelerometerCalibrator(
21989                     invalidGravityNorm, true, biasX, biasY, biasZ);
21990             fail("IllegalArgumentException expected but not thrown");
21991         } catch (final IllegalArgumentException ignore) {
21992         }
21993         assertNull(calibrator);
21994     }
21995 
21996     @Test
21997     public void testConstructor170() throws WrongSizeException {
21998         final Matrix ba = generateBa();
21999         final double biasX = ba.getElementAtIndex(0);
22000         final double biasY = ba.getElementAtIndex(1);
22001         final double biasZ = ba.getElementAtIndex(2);
22002 
22003         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22004         final double latitude = Math.toRadians(
22005                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22006         final double longitude = Math.toRadians(
22007                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22008         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22009         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22010         final NEDVelocity nedVelocity = new NEDVelocity();
22011         final ECEFPosition ecefPosition = new ECEFPosition();
22012         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22013         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22014                 ecefPosition, ecefVelocity);
22015         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22016                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22017         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22018 
22019         KnownGravityNormAccelerometerCalibrator calibrator =
22020                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
22021                         true, biasX, biasY, biasZ, this);
22022 
22023         // check default values
22024         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22025         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22026         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22027         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22028         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22029         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22030         final Acceleration bx2 = new Acceleration(0.0,
22031                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22032         calibrator.getInitialBiasXAsAcceleration(bx2);
22033         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22034         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22035         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22036         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22037         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22038         final Acceleration by2 = new Acceleration(0.0,
22039                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22040         calibrator.getInitialBiasYAsAcceleration(by2);
22041         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22042         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22043         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22044         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22045         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22046         final Acceleration bz2 = new Acceleration(0.0,
22047                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22048         calibrator.getInitialBiasZAsAcceleration(bz2);
22049         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22050         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22051         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22052         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22053         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22054         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22055         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22056         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22057         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22058         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22059         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22060         final double[] bias1 = calibrator.getInitialBias();
22061         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22062         final double[] bias2 = new double[3];
22063         calibrator.getInitialBias(bias2);
22064         assertArrayEquals(bias1, bias2, 0.0);
22065         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22066         assertEquals(b1, ba);
22067         final Matrix b2 = new Matrix(3, 1);
22068         calibrator.getInitialBiasAsMatrix(b2);
22069         assertEquals(b1, b2);
22070         final Matrix ma1 = calibrator.getInitialMa();
22071         assertEquals(ma1, new Matrix(3, 3));
22072         final Matrix ma2 = new Matrix(3, 3);
22073         calibrator.getInitialMa(ma2);
22074         assertEquals(ma1, ma2);
22075         assertNull(calibrator.getMeasurements());
22076         assertTrue(calibrator.isCommonAxisUsed());
22077         assertSame(calibrator.getListener(), this);
22078         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22079         assertFalse(calibrator.isReady());
22080         assertFalse(calibrator.isRunning());
22081         assertNull(calibrator.getEstimatedBiases());
22082         assertFalse(calibrator.getEstimatedBiases(null));
22083         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22084         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22085         assertNull(calibrator.getEstimatedBiasFx());
22086         assertNull(calibrator.getEstimatedBiasFy());
22087         assertNull(calibrator.getEstimatedBiasFz());
22088         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22089         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22090         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22091         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22092         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22093         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22094         assertNull(calibrator.getEstimatedMa());
22095         assertNull(calibrator.getEstimatedSx());
22096         assertNull(calibrator.getEstimatedSy());
22097         assertNull(calibrator.getEstimatedSz());
22098         assertNull(calibrator.getEstimatedMxy());
22099         assertNull(calibrator.getEstimatedMxz());
22100         assertNull(calibrator.getEstimatedMyx());
22101         assertNull(calibrator.getEstimatedMyz());
22102         assertNull(calibrator.getEstimatedMzx());
22103         assertNull(calibrator.getEstimatedMzy());
22104         assertNull(calibrator.getEstimatedCovariance());
22105         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22106         assertNotNull(calibrator.getGroundTruthGravityNorm());
22107         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22108         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22109         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22110                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22111         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22112         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22113         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22114 
22115         // Force IllegalArgumentException
22116         final Acceleration invalidGravityNorm = new Acceleration(
22117                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22118 
22119         calibrator = null;
22120         try {
22121             calibrator = new KnownGravityNormAccelerometerCalibrator(
22122                     invalidGravityNorm, true,
22123                     biasX, biasY, biasZ, this);
22124             fail("IllegalArgumentException expected but not thrown");
22125         } catch (final IllegalArgumentException ignore) {
22126         }
22127         assertNull(calibrator);
22128     }
22129 
22130     @Test
22131     public void testConstructor171() throws WrongSizeException {
22132         final Collection<StandardDeviationBodyKinematics> measurements =
22133                 Collections.emptyList();
22134 
22135         final Matrix ba = generateBa();
22136         final double biasX = ba.getElementAtIndex(0);
22137         final double biasY = ba.getElementAtIndex(1);
22138         final double biasZ = ba.getElementAtIndex(2);
22139 
22140         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22141         final double latitude = Math.toRadians(
22142                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22143         final double longitude = Math.toRadians(
22144                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22145         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22146         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22147         final NEDVelocity nedVelocity = new NEDVelocity();
22148         final ECEFPosition ecefPosition = new ECEFPosition();
22149         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22150         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22151                 ecefPosition, ecefVelocity);
22152         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22153                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22154         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22155 
22156         KnownGravityNormAccelerometerCalibrator calibrator =
22157                 new KnownGravityNormAccelerometerCalibrator(
22158                         gravityNorm, measurements,
22159                         true, biasX, biasY, biasZ);
22160 
22161         // check default values
22162         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22163         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22164         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22165         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22166         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22167         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22168         final Acceleration bx2 = new Acceleration(0.0,
22169                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22170         calibrator.getInitialBiasXAsAcceleration(bx2);
22171         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22172         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22173         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22174         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22175         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22176         final Acceleration by2 = new Acceleration(0.0,
22177                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22178         calibrator.getInitialBiasYAsAcceleration(by2);
22179         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22180         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22181         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22182         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22183         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22184         final Acceleration bz2 = new Acceleration(0.0,
22185                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22186         calibrator.getInitialBiasZAsAcceleration(bz2);
22187         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22188         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22189         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22190         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22191         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22192         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22193         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22194         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22195         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22196         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22197         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22198         final double[] bias1 = calibrator.getInitialBias();
22199         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22200         final double[] bias2 = new double[3];
22201         calibrator.getInitialBias(bias2);
22202         assertArrayEquals(bias1, bias2, 0.0);
22203         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22204         assertEquals(b1, ba);
22205         final Matrix b2 = new Matrix(3, 1);
22206         calibrator.getInitialBiasAsMatrix(b2);
22207         assertEquals(b1, b2);
22208         final Matrix ma1 = calibrator.getInitialMa();
22209         assertEquals(ma1, new Matrix(3, 3));
22210         final Matrix ma2 = new Matrix(3, 3);
22211         calibrator.getInitialMa(ma2);
22212         assertEquals(ma1, ma2);
22213         assertSame(calibrator.getMeasurements(), measurements);
22214         assertTrue(calibrator.isCommonAxisUsed());
22215         assertNull(calibrator.getListener());
22216         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22217         assertFalse(calibrator.isReady());
22218         assertFalse(calibrator.isRunning());
22219         assertNull(calibrator.getEstimatedBiases());
22220         assertFalse(calibrator.getEstimatedBiases(null));
22221         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22222         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22223         assertNull(calibrator.getEstimatedBiasFx());
22224         assertNull(calibrator.getEstimatedBiasFy());
22225         assertNull(calibrator.getEstimatedBiasFz());
22226         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22227         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22228         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22229         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22230         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22231         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22232         assertNull(calibrator.getEstimatedMa());
22233         assertNull(calibrator.getEstimatedSx());
22234         assertNull(calibrator.getEstimatedSy());
22235         assertNull(calibrator.getEstimatedSz());
22236         assertNull(calibrator.getEstimatedMxy());
22237         assertNull(calibrator.getEstimatedMxz());
22238         assertNull(calibrator.getEstimatedMyx());
22239         assertNull(calibrator.getEstimatedMyz());
22240         assertNull(calibrator.getEstimatedMzx());
22241         assertNull(calibrator.getEstimatedMzy());
22242         assertNull(calibrator.getEstimatedCovariance());
22243         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22244         assertNotNull(calibrator.getGroundTruthGravityNorm());
22245         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22246         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22247         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22248                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22249         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22250         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22251         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22252 
22253         // Force IllegalArgumentException
22254         final Acceleration invalidGravityNorm = new Acceleration(
22255                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22256 
22257         calibrator = null;
22258         try {
22259             calibrator = new KnownGravityNormAccelerometerCalibrator(
22260                     invalidGravityNorm, measurements,
22261                     true, biasX, biasY, biasZ);
22262             fail("IllegalArgumentException expected but not thrown");
22263         } catch (final IllegalArgumentException ignore) {
22264         }
22265         assertNull(calibrator);
22266     }
22267 
22268     @Test
22269     public void testConstructor172() throws WrongSizeException {
22270         final Collection<StandardDeviationBodyKinematics> measurements =
22271                 Collections.emptyList();
22272 
22273         final Matrix ba = generateBa();
22274         final double biasX = ba.getElementAtIndex(0);
22275         final double biasY = ba.getElementAtIndex(1);
22276         final double biasZ = ba.getElementAtIndex(2);
22277 
22278         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22279         final double latitude = Math.toRadians(
22280                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22281         final double longitude = Math.toRadians(
22282                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22283         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22284         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22285         final NEDVelocity nedVelocity = new NEDVelocity();
22286         final ECEFPosition ecefPosition = new ECEFPosition();
22287         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22288         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22289                 ecefPosition, ecefVelocity);
22290         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22291                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22292         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22293 
22294         KnownGravityNormAccelerometerCalibrator calibrator =
22295                 new KnownGravityNormAccelerometerCalibrator(
22296                         gravityNorm, measurements,
22297                         true, biasX, biasY, biasZ, this);
22298 
22299         // check default values
22300         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22301         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22302         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22303         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22304         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22305         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22306         final Acceleration bx2 = new Acceleration(0.0,
22307                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22308         calibrator.getInitialBiasXAsAcceleration(bx2);
22309         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22310         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22311         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22312         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22313         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22314         final Acceleration by2 = new Acceleration(0.0,
22315                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22316         calibrator.getInitialBiasYAsAcceleration(by2);
22317         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22318         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22319         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22320         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22321         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22322         final Acceleration bz2 = new Acceleration(0.0,
22323                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22324         calibrator.getInitialBiasZAsAcceleration(bz2);
22325         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22326         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22327         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22328         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22329         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22330         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22331         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22332         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22333         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22334         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22335         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22336         final double[] bias1 = calibrator.getInitialBias();
22337         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22338         final double[] bias2 = new double[3];
22339         calibrator.getInitialBias(bias2);
22340         assertArrayEquals(bias1, bias2, 0.0);
22341         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22342         assertEquals(b1, ba);
22343         final Matrix b2 = new Matrix(3, 1);
22344         calibrator.getInitialBiasAsMatrix(b2);
22345         assertEquals(b1, b2);
22346         final Matrix ma1 = calibrator.getInitialMa();
22347         assertEquals(ma1, new Matrix(3, 3));
22348         final Matrix ma2 = new Matrix(3, 3);
22349         calibrator.getInitialMa(ma2);
22350         assertEquals(ma1, ma2);
22351         assertSame(calibrator.getMeasurements(), measurements);
22352         assertTrue(calibrator.isCommonAxisUsed());
22353         assertSame(calibrator.getListener(), this);
22354         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22355         assertFalse(calibrator.isReady());
22356         assertFalse(calibrator.isRunning());
22357         assertNull(calibrator.getEstimatedBiases());
22358         assertFalse(calibrator.getEstimatedBiases(null));
22359         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22360         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22361         assertNull(calibrator.getEstimatedBiasFx());
22362         assertNull(calibrator.getEstimatedBiasFy());
22363         assertNull(calibrator.getEstimatedBiasFz());
22364         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22365         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22366         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22367         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22368         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22369         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22370         assertNull(calibrator.getEstimatedMa());
22371         assertNull(calibrator.getEstimatedSx());
22372         assertNull(calibrator.getEstimatedSy());
22373         assertNull(calibrator.getEstimatedSz());
22374         assertNull(calibrator.getEstimatedMxy());
22375         assertNull(calibrator.getEstimatedMxz());
22376         assertNull(calibrator.getEstimatedMyx());
22377         assertNull(calibrator.getEstimatedMyz());
22378         assertNull(calibrator.getEstimatedMzx());
22379         assertNull(calibrator.getEstimatedMzy());
22380         assertNull(calibrator.getEstimatedCovariance());
22381         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22382         assertNotNull(calibrator.getGroundTruthGravityNorm());
22383         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22384         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22385         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22386                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22387         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22388         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22389         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22390 
22391         // Force IllegalArgumentException
22392         final Acceleration invalidGravityNorm = new Acceleration(
22393                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22394 
22395         calibrator = null;
22396         try {
22397             calibrator = new KnownGravityNormAccelerometerCalibrator(
22398                     invalidGravityNorm, measurements,
22399                     true, biasX, biasY, biasZ, this);
22400             fail("IllegalArgumentException expected but not thrown");
22401         } catch (final IllegalArgumentException ignore) {
22402         }
22403         assertNull(calibrator);
22404     }
22405 
22406     @Test
22407     public void testConstructor173() throws WrongSizeException {
22408         final Matrix ba = generateBa();
22409         final double biasX = ba.getElementAtIndex(0);
22410         final double biasY = ba.getElementAtIndex(1);
22411         final double biasZ = ba.getElementAtIndex(2);
22412 
22413         final Acceleration bx = new Acceleration(biasX,
22414                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22415         final Acceleration by = new Acceleration(biasY,
22416                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22417         final Acceleration bz = new Acceleration(biasZ,
22418                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22419 
22420         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22421         final double latitude = Math.toRadians(
22422                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22423         final double longitude = Math.toRadians(
22424                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22425         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22426         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22427         final NEDVelocity nedVelocity = new NEDVelocity();
22428         final ECEFPosition ecefPosition = new ECEFPosition();
22429         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22430         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22431                 ecefPosition, ecefVelocity);
22432         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22433                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22434         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22435 
22436         KnownGravityNormAccelerometerCalibrator calibrator =
22437                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
22438                         bx, by, bz);
22439 
22440         // check default values
22441         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22442         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22443         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22444         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22445         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22446         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22447         final Acceleration bx2 = new Acceleration(0.0,
22448                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22449         calibrator.getInitialBiasXAsAcceleration(bx2);
22450         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22451         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22452         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22453         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22454         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22455         final Acceleration by2 = new Acceleration(0.0,
22456                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22457         calibrator.getInitialBiasYAsAcceleration(by2);
22458         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22459         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22460         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22461         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22462         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22463         final Acceleration bz2 = new Acceleration(0.0,
22464                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22465         calibrator.getInitialBiasZAsAcceleration(bz2);
22466         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22467         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22468         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22469         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22470         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22471         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22472         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22473         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22474         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22475         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22476         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22477         final double[] bias1 = calibrator.getInitialBias();
22478         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22479         final double[] bias2 = new double[3];
22480         calibrator.getInitialBias(bias2);
22481         assertArrayEquals(bias1, bias2, 0.0);
22482         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22483         assertEquals(b1, ba);
22484         final Matrix b2 = new Matrix(3, 1);
22485         calibrator.getInitialBiasAsMatrix(b2);
22486         assertEquals(b1, b2);
22487         final Matrix ma1 = calibrator.getInitialMa();
22488         assertEquals(ma1, new Matrix(3, 3));
22489         final Matrix ma2 = new Matrix(3, 3);
22490         calibrator.getInitialMa(ma2);
22491         assertEquals(ma1, ma2);
22492         assertNull(calibrator.getMeasurements());
22493         assertFalse(calibrator.isCommonAxisUsed());
22494         assertNull(calibrator.getListener());
22495         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22496         assertFalse(calibrator.isReady());
22497         assertFalse(calibrator.isRunning());
22498         assertNull(calibrator.getEstimatedBiases());
22499         assertFalse(calibrator.getEstimatedBiases(null));
22500         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22501         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22502         assertNull(calibrator.getEstimatedBiasFx());
22503         assertNull(calibrator.getEstimatedBiasFy());
22504         assertNull(calibrator.getEstimatedBiasFz());
22505         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22506         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22507         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22508         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22509         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22510         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22511         assertNull(calibrator.getEstimatedMa());
22512         assertNull(calibrator.getEstimatedSx());
22513         assertNull(calibrator.getEstimatedSy());
22514         assertNull(calibrator.getEstimatedSz());
22515         assertNull(calibrator.getEstimatedMxy());
22516         assertNull(calibrator.getEstimatedMxz());
22517         assertNull(calibrator.getEstimatedMyx());
22518         assertNull(calibrator.getEstimatedMyz());
22519         assertNull(calibrator.getEstimatedMzx());
22520         assertNull(calibrator.getEstimatedMzy());
22521         assertNull(calibrator.getEstimatedCovariance());
22522         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22523         assertNotNull(calibrator.getGroundTruthGravityNorm());
22524         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22525         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22526         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22527                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22528         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22529         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22530         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22531 
22532         // Force IllegalArgumentException
22533         final Acceleration invalidGravityNorm = new Acceleration(
22534                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22535 
22536         calibrator = null;
22537         try {
22538             calibrator = new KnownGravityNormAccelerometerCalibrator(
22539                     invalidGravityNorm, bx, by, bz);
22540             fail("IllegalArgumentException expected but not thrown");
22541         } catch (final IllegalArgumentException ignore) {
22542         }
22543         assertNull(calibrator);
22544     }
22545 
22546     @Test
22547     public void testConstructor174() throws WrongSizeException {
22548         final Matrix ba = generateBa();
22549         final double biasX = ba.getElementAtIndex(0);
22550         final double biasY = ba.getElementAtIndex(1);
22551         final double biasZ = ba.getElementAtIndex(2);
22552 
22553         final Acceleration bx = new Acceleration(biasX,
22554                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22555         final Acceleration by = new Acceleration(biasY,
22556                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22557         final Acceleration bz = new Acceleration(biasZ,
22558                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22559 
22560         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22561         final double latitude = Math.toRadians(
22562                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22563         final double longitude = Math.toRadians(
22564                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22565         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22566         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22567         final NEDVelocity nedVelocity = new NEDVelocity();
22568         final ECEFPosition ecefPosition = new ECEFPosition();
22569         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22570         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22571                 ecefPosition, ecefVelocity);
22572         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22573                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22574         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22575 
22576         KnownGravityNormAccelerometerCalibrator calibrator =
22577                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
22578                         bx, by, bz, this);
22579 
22580         // check default values
22581         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22582         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22583         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22584         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22585         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22586         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22587         final Acceleration bx2 = new Acceleration(0.0,
22588                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22589         calibrator.getInitialBiasXAsAcceleration(bx2);
22590         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22591         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22592         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22593         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22594         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22595         final Acceleration by2 = new Acceleration(0.0,
22596                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22597         calibrator.getInitialBiasYAsAcceleration(by2);
22598         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22599         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22600         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22601         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22602         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22603         final Acceleration bz2 = new Acceleration(0.0,
22604                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22605         calibrator.getInitialBiasZAsAcceleration(bz2);
22606         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22607         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22608         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22609         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22610         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22611         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22612         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22613         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22614         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22615         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22616         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22617         final double[] bias1 = calibrator.getInitialBias();
22618         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22619         final double[] bias2 = new double[3];
22620         calibrator.getInitialBias(bias2);
22621         assertArrayEquals(bias1, bias2, 0.0);
22622         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22623         assertEquals(b1, ba);
22624         final Matrix b2 = new Matrix(3, 1);
22625         calibrator.getInitialBiasAsMatrix(b2);
22626         assertEquals(b1, b2);
22627         final Matrix ma1 = calibrator.getInitialMa();
22628         assertEquals(ma1, new Matrix(3, 3));
22629         final Matrix ma2 = new Matrix(3, 3);
22630         calibrator.getInitialMa(ma2);
22631         assertEquals(ma1, ma2);
22632         assertNull(calibrator.getMeasurements());
22633         assertFalse(calibrator.isCommonAxisUsed());
22634         assertSame(calibrator.getListener(), this);
22635         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22636         assertFalse(calibrator.isReady());
22637         assertFalse(calibrator.isRunning());
22638         assertNull(calibrator.getEstimatedBiases());
22639         assertFalse(calibrator.getEstimatedBiases(null));
22640         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22641         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22642         assertNull(calibrator.getEstimatedBiasFx());
22643         assertNull(calibrator.getEstimatedBiasFy());
22644         assertNull(calibrator.getEstimatedBiasFz());
22645         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22646         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22647         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22648         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22649         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22650         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22651         assertNull(calibrator.getEstimatedMa());
22652         assertNull(calibrator.getEstimatedSx());
22653         assertNull(calibrator.getEstimatedSy());
22654         assertNull(calibrator.getEstimatedSz());
22655         assertNull(calibrator.getEstimatedMxy());
22656         assertNull(calibrator.getEstimatedMxz());
22657         assertNull(calibrator.getEstimatedMyx());
22658         assertNull(calibrator.getEstimatedMyz());
22659         assertNull(calibrator.getEstimatedMzx());
22660         assertNull(calibrator.getEstimatedMzy());
22661         assertNull(calibrator.getEstimatedCovariance());
22662         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22663         assertNotNull(calibrator.getGroundTruthGravityNorm());
22664         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22665         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22666         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22667                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22668         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22669         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22670         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22671 
22672         // Force IllegalArgumentException
22673         final Acceleration invalidGravityNorm = new Acceleration(
22674                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22675 
22676         calibrator = null;
22677         try {
22678             calibrator = new KnownGravityNormAccelerometerCalibrator(
22679                     invalidGravityNorm, bx, by, bz, this);
22680             fail("IllegalArgumentException expected but not thrown");
22681         } catch (final IllegalArgumentException ignore) {
22682         }
22683         assertNull(calibrator);
22684     }
22685 
22686     @Test
22687     public void testConstructor175() throws WrongSizeException {
22688         final Collection<StandardDeviationBodyKinematics> measurements =
22689                 Collections.emptyList();
22690 
22691         final Matrix ba = generateBa();
22692         final double biasX = ba.getElementAtIndex(0);
22693         final double biasY = ba.getElementAtIndex(1);
22694         final double biasZ = ba.getElementAtIndex(2);
22695 
22696         final Acceleration bx = new Acceleration(biasX,
22697                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22698         final Acceleration by = new Acceleration(biasY,
22699                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22700         final Acceleration bz = new Acceleration(biasZ,
22701                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22702 
22703         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22704         final double latitude = Math.toRadians(
22705                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22706         final double longitude = Math.toRadians(
22707                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22708         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22709         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22710         final NEDVelocity nedVelocity = new NEDVelocity();
22711         final ECEFPosition ecefPosition = new ECEFPosition();
22712         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22713         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22714                 ecefPosition, ecefVelocity);
22715         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22716                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22717         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22718 
22719         KnownGravityNormAccelerometerCalibrator calibrator =
22720                 new KnownGravityNormAccelerometerCalibrator(
22721                         gravityNorm, measurements, bx, by, bz);
22722 
22723         // check default values
22724         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22725         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22726         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22727         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22728         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22729         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22730         final Acceleration bx2 = new Acceleration(0.0,
22731                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22732         calibrator.getInitialBiasXAsAcceleration(bx2);
22733         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22734         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22735         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22736         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22737         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22738         final Acceleration by2 = new Acceleration(0.0,
22739                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22740         calibrator.getInitialBiasYAsAcceleration(by2);
22741         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22742         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22743         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22744         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22745         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22746         final Acceleration bz2 = new Acceleration(0.0,
22747                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22748         calibrator.getInitialBiasZAsAcceleration(bz2);
22749         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22750         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22751         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22752         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22753         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22754         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22755         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22756         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22757         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22758         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22759         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22760         final double[] bias1 = calibrator.getInitialBias();
22761         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22762         final double[] bias2 = new double[3];
22763         calibrator.getInitialBias(bias2);
22764         assertArrayEquals(bias1, bias2, 0.0);
22765         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22766         assertEquals(b1, ba);
22767         final Matrix b2 = new Matrix(3, 1);
22768         calibrator.getInitialBiasAsMatrix(b2);
22769         assertEquals(b1, b2);
22770         final Matrix ma1 = calibrator.getInitialMa();
22771         assertEquals(ma1, new Matrix(3, 3));
22772         final Matrix ma2 = new Matrix(3, 3);
22773         calibrator.getInitialMa(ma2);
22774         assertEquals(ma1, ma2);
22775         assertSame(calibrator.getMeasurements(), measurements);
22776         assertFalse(calibrator.isCommonAxisUsed());
22777         assertNull(calibrator.getListener());
22778         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22779         assertFalse(calibrator.isReady());
22780         assertFalse(calibrator.isRunning());
22781         assertNull(calibrator.getEstimatedBiases());
22782         assertFalse(calibrator.getEstimatedBiases(null));
22783         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22784         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22785         assertNull(calibrator.getEstimatedBiasFx());
22786         assertNull(calibrator.getEstimatedBiasFy());
22787         assertNull(calibrator.getEstimatedBiasFz());
22788         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22789         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22790         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22791         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22792         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22793         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22794         assertNull(calibrator.getEstimatedMa());
22795         assertNull(calibrator.getEstimatedSx());
22796         assertNull(calibrator.getEstimatedSy());
22797         assertNull(calibrator.getEstimatedSz());
22798         assertNull(calibrator.getEstimatedMxy());
22799         assertNull(calibrator.getEstimatedMxz());
22800         assertNull(calibrator.getEstimatedMyx());
22801         assertNull(calibrator.getEstimatedMyz());
22802         assertNull(calibrator.getEstimatedMzx());
22803         assertNull(calibrator.getEstimatedMzy());
22804         assertNull(calibrator.getEstimatedCovariance());
22805         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22806         assertNotNull(calibrator.getGroundTruthGravityNorm());
22807         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22808         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22809         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22810                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22811         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22812         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22813         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22814 
22815         // Force IllegalArgumentException
22816         final Acceleration invalidGravityNorm = new Acceleration(
22817                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22818 
22819         calibrator = null;
22820         try {
22821             calibrator = new KnownGravityNormAccelerometerCalibrator(
22822                     invalidGravityNorm, measurements, bx, by, bz);
22823             fail("IllegalArgumentException expected but not thrown");
22824         } catch (final IllegalArgumentException ignore) {
22825         }
22826         assertNull(calibrator);
22827     }
22828 
22829     @Test
22830     public void testConstructor176() throws WrongSizeException {
22831         final Collection<StandardDeviationBodyKinematics> measurements =
22832                 Collections.emptyList();
22833 
22834         final Matrix ba = generateBa();
22835         final double biasX = ba.getElementAtIndex(0);
22836         final double biasY = ba.getElementAtIndex(1);
22837         final double biasZ = ba.getElementAtIndex(2);
22838 
22839         final Acceleration bx = new Acceleration(biasX,
22840                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22841         final Acceleration by = new Acceleration(biasY,
22842                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22843         final Acceleration bz = new Acceleration(biasZ,
22844                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22845 
22846         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22847         final double latitude = Math.toRadians(
22848                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22849         final double longitude = Math.toRadians(
22850                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22851         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22852         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22853         final NEDVelocity nedVelocity = new NEDVelocity();
22854         final ECEFPosition ecefPosition = new ECEFPosition();
22855         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22856         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22857                 ecefPosition, ecefVelocity);
22858         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22859                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22860         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
22861 
22862         KnownGravityNormAccelerometerCalibrator calibrator =
22863                 new KnownGravityNormAccelerometerCalibrator(
22864                         gravityNorm, measurements,
22865                         bx, by, bz, this);
22866 
22867         // check default values
22868         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22869         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22870         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22871         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22872         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22873         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22874         final Acceleration bx2 = new Acceleration(0.0,
22875                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22876         calibrator.getInitialBiasXAsAcceleration(bx2);
22877         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22878         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22879         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22880         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22881         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22882         final Acceleration by2 = new Acceleration(0.0,
22883                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22884         calibrator.getInitialBiasYAsAcceleration(by2);
22885         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22886         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22887         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22888         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22889         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22890         final Acceleration bz2 = new Acceleration(0.0,
22891                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22892         calibrator.getInitialBiasZAsAcceleration(bz2);
22893         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22894         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22895         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22896         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22897         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22898         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22899         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22900         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22901         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22902         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22903         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22904         final double[] bias1 = calibrator.getInitialBias();
22905         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22906         final double[] bias2 = new double[3];
22907         calibrator.getInitialBias(bias2);
22908         assertArrayEquals(bias1, bias2, 0.0);
22909         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22910         assertEquals(b1, ba);
22911         final Matrix b2 = new Matrix(3, 1);
22912         calibrator.getInitialBiasAsMatrix(b2);
22913         assertEquals(b1, b2);
22914         final Matrix ma1 = calibrator.getInitialMa();
22915         assertEquals(ma1, new Matrix(3, 3));
22916         final Matrix ma2 = new Matrix(3, 3);
22917         calibrator.getInitialMa(ma2);
22918         assertEquals(ma1, ma2);
22919         assertSame(calibrator.getMeasurements(), measurements);
22920         assertFalse(calibrator.isCommonAxisUsed());
22921         assertSame(calibrator.getListener(), this);
22922         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22923         assertFalse(calibrator.isReady());
22924         assertFalse(calibrator.isRunning());
22925         assertNull(calibrator.getEstimatedBiases());
22926         assertFalse(calibrator.getEstimatedBiases(null));
22927         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22928         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22929         assertNull(calibrator.getEstimatedBiasFx());
22930         assertNull(calibrator.getEstimatedBiasFy());
22931         assertNull(calibrator.getEstimatedBiasFz());
22932         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22933         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22934         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22935         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22936         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22937         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22938         assertNull(calibrator.getEstimatedMa());
22939         assertNull(calibrator.getEstimatedSx());
22940         assertNull(calibrator.getEstimatedSy());
22941         assertNull(calibrator.getEstimatedSz());
22942         assertNull(calibrator.getEstimatedMxy());
22943         assertNull(calibrator.getEstimatedMxz());
22944         assertNull(calibrator.getEstimatedMyx());
22945         assertNull(calibrator.getEstimatedMyz());
22946         assertNull(calibrator.getEstimatedMzx());
22947         assertNull(calibrator.getEstimatedMzy());
22948         assertNull(calibrator.getEstimatedCovariance());
22949         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22950         assertNotNull(calibrator.getGroundTruthGravityNorm());
22951         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22952         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22953         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22954                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22955         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22956         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22957         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22958 
22959         // Force IllegalArgumentException
22960         final Acceleration invalidGravityNorm = new Acceleration(
22961                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22962 
22963         calibrator = null;
22964         try {
22965             calibrator = new KnownGravityNormAccelerometerCalibrator(
22966                     invalidGravityNorm, measurements,
22967                     bx, by, bz, this);
22968             fail("IllegalArgumentException expected but not thrown");
22969         } catch (final IllegalArgumentException ignore) {
22970         }
22971         assertNull(calibrator);
22972     }
22973 
22974     @Test
22975     public void testConstructor177() throws WrongSizeException {
22976         final Matrix ba = generateBa();
22977         final double biasX = ba.getElementAtIndex(0);
22978         final double biasY = ba.getElementAtIndex(1);
22979         final double biasZ = ba.getElementAtIndex(2);
22980 
22981         final Acceleration bx = new Acceleration(biasX,
22982                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22983         final Acceleration by = new Acceleration(biasY,
22984                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22985         final Acceleration bz = new Acceleration(biasZ,
22986                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22987 
22988         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22989         final double latitude = Math.toRadians(
22990                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22991         final double longitude = Math.toRadians(
22992                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22993         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22994         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22995         final NEDVelocity nedVelocity = new NEDVelocity();
22996         final ECEFPosition ecefPosition = new ECEFPosition();
22997         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22998         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22999                 ecefPosition, ecefVelocity);
23000         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23001                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23002         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23003 
23004         KnownGravityNormAccelerometerCalibrator calibrator =
23005                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
23006                         true, bx, by, bz);
23007 
23008         // check default values
23009         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23010         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23011         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23012         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23013         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23014         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23015         final Acceleration bx2 = new Acceleration(0.0,
23016                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23017         calibrator.getInitialBiasXAsAcceleration(bx2);
23018         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23019         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23020         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23021         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23022         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23023         final Acceleration by2 = new Acceleration(0.0,
23024                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23025         calibrator.getInitialBiasYAsAcceleration(by2);
23026         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23027         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23028         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23029         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23030         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23031         final Acceleration bz2 = new Acceleration(0.0,
23032                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23033         calibrator.getInitialBiasZAsAcceleration(bz2);
23034         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23035         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23036         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
23037         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
23038         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
23039         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23040         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23041         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23042         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23043         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23044         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23045         final double[] bias1 = calibrator.getInitialBias();
23046         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23047         final double[] bias2 = new double[3];
23048         calibrator.getInitialBias(bias2);
23049         assertArrayEquals(bias1, bias2, 0.0);
23050         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23051         assertEquals(b1, ba);
23052         final Matrix b2 = new Matrix(3, 1);
23053         calibrator.getInitialBiasAsMatrix(b2);
23054         assertEquals(b1, b2);
23055         final Matrix ma1 = calibrator.getInitialMa();
23056         assertEquals(ma1, new Matrix(3, 3));
23057         final Matrix ma2 = new Matrix(3, 3);
23058         calibrator.getInitialMa(ma2);
23059         assertEquals(ma1, ma2);
23060         assertNull(calibrator.getMeasurements());
23061         assertTrue(calibrator.isCommonAxisUsed());
23062         assertNull(calibrator.getListener());
23063         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23064         assertFalse(calibrator.isReady());
23065         assertFalse(calibrator.isRunning());
23066         assertNull(calibrator.getEstimatedBiases());
23067         assertFalse(calibrator.getEstimatedBiases(null));
23068         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23069         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23070         assertNull(calibrator.getEstimatedBiasFx());
23071         assertNull(calibrator.getEstimatedBiasFy());
23072         assertNull(calibrator.getEstimatedBiasFz());
23073         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23074         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23075         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23076         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23077         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23078         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23079         assertNull(calibrator.getEstimatedMa());
23080         assertNull(calibrator.getEstimatedSx());
23081         assertNull(calibrator.getEstimatedSy());
23082         assertNull(calibrator.getEstimatedSz());
23083         assertNull(calibrator.getEstimatedMxy());
23084         assertNull(calibrator.getEstimatedMxz());
23085         assertNull(calibrator.getEstimatedMyx());
23086         assertNull(calibrator.getEstimatedMyz());
23087         assertNull(calibrator.getEstimatedMzx());
23088         assertNull(calibrator.getEstimatedMzy());
23089         assertNull(calibrator.getEstimatedCovariance());
23090         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23091         assertNotNull(calibrator.getGroundTruthGravityNorm());
23092         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23093         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23094         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23095                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23096         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23097         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23098         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23099 
23100         // Force IllegalArgumentException
23101         final Acceleration invalidGravityNorm = new Acceleration(
23102                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23103 
23104         calibrator = null;
23105         try {
23106             calibrator = new KnownGravityNormAccelerometerCalibrator(
23107                     invalidGravityNorm, true,
23108                     bx, by, bz);
23109             fail("IllegalArgumentException expected but not thrown");
23110         } catch (final IllegalArgumentException ignore) {
23111         }
23112         assertNull(calibrator);
23113     }
23114 
23115     @Test
23116     public void testConstructor178() throws WrongSizeException {
23117         final Matrix ba = generateBa();
23118         final double biasX = ba.getElementAtIndex(0);
23119         final double biasY = ba.getElementAtIndex(1);
23120         final double biasZ = ba.getElementAtIndex(2);
23121 
23122         final Acceleration bx = new Acceleration(biasX,
23123                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23124         final Acceleration by = new Acceleration(biasY,
23125                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23126         final Acceleration bz = new Acceleration(biasZ,
23127                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23128 
23129         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23130         final double latitude = Math.toRadians(
23131                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23132         final double longitude = Math.toRadians(
23133                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23134         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23135         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23136         final NEDVelocity nedVelocity = new NEDVelocity();
23137         final ECEFPosition ecefPosition = new ECEFPosition();
23138         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23139         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23140                 ecefPosition, ecefVelocity);
23141         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23142                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23143         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23144 
23145         KnownGravityNormAccelerometerCalibrator calibrator =
23146                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
23147                         true, bx, by, bz, this);
23148 
23149         // check default values
23150         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23151         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23152         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23153         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23154         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23155         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23156         final Acceleration bx2 = new Acceleration(0.0,
23157                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23158         calibrator.getInitialBiasXAsAcceleration(bx2);
23159         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23160         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23161         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23162         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23163         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23164         final Acceleration by2 = new Acceleration(0.0,
23165                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23166         calibrator.getInitialBiasYAsAcceleration(by2);
23167         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23168         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23169         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23170         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23171         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23172         final Acceleration bz2 = new Acceleration(0.0,
23173                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23174         calibrator.getInitialBiasZAsAcceleration(bz2);
23175         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23176         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23177         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
23178         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
23179         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
23180         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23181         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23182         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23183         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23184         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23185         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23186         final double[] bias1 = calibrator.getInitialBias();
23187         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23188         final double[] bias2 = new double[3];
23189         calibrator.getInitialBias(bias2);
23190         assertArrayEquals(bias1, bias2, 0.0);
23191         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23192         assertEquals(b1, ba);
23193         final Matrix b2 = new Matrix(3, 1);
23194         calibrator.getInitialBiasAsMatrix(b2);
23195         assertEquals(b1, b2);
23196         final Matrix ma1 = calibrator.getInitialMa();
23197         assertEquals(ma1, new Matrix(3, 3));
23198         final Matrix ma2 = new Matrix(3, 3);
23199         calibrator.getInitialMa(ma2);
23200         assertEquals(ma1, ma2);
23201         assertNull(calibrator.getMeasurements());
23202         assertTrue(calibrator.isCommonAxisUsed());
23203         assertSame(calibrator.getListener(), this);
23204         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23205         assertFalse(calibrator.isReady());
23206         assertFalse(calibrator.isRunning());
23207         assertNull(calibrator.getEstimatedBiases());
23208         assertFalse(calibrator.getEstimatedBiases(null));
23209         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23210         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23211         assertNull(calibrator.getEstimatedBiasFx());
23212         assertNull(calibrator.getEstimatedBiasFy());
23213         assertNull(calibrator.getEstimatedBiasFz());
23214         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23215         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23216         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23217         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23218         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23219         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23220         assertNull(calibrator.getEstimatedMa());
23221         assertNull(calibrator.getEstimatedSx());
23222         assertNull(calibrator.getEstimatedSy());
23223         assertNull(calibrator.getEstimatedSz());
23224         assertNull(calibrator.getEstimatedMxy());
23225         assertNull(calibrator.getEstimatedMxz());
23226         assertNull(calibrator.getEstimatedMyx());
23227         assertNull(calibrator.getEstimatedMyz());
23228         assertNull(calibrator.getEstimatedMzx());
23229         assertNull(calibrator.getEstimatedMzy());
23230         assertNull(calibrator.getEstimatedCovariance());
23231         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23232         assertNotNull(calibrator.getGroundTruthGravityNorm());
23233         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23234         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23235         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23236                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23237         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23238         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23239         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23240 
23241         // Force IllegalArgumentException
23242         final Acceleration invalidGravityNorm = new Acceleration(
23243                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23244 
23245         calibrator = null;
23246         try {
23247             calibrator = new KnownGravityNormAccelerometerCalibrator(
23248                     invalidGravityNorm, true,
23249                     bx, by, bz, this);
23250             fail("IllegalArgumentException expected but not thrown");
23251         } catch (final IllegalArgumentException ignore) {
23252         }
23253         assertNull(calibrator);
23254     }
23255 
23256     @Test
23257     public void testConstructor179() throws WrongSizeException {
23258         final Collection<StandardDeviationBodyKinematics> measurements =
23259                 Collections.emptyList();
23260 
23261         final Matrix ba = generateBa();
23262         final double biasX = ba.getElementAtIndex(0);
23263         final double biasY = ba.getElementAtIndex(1);
23264         final double biasZ = ba.getElementAtIndex(2);
23265 
23266         final Acceleration bx = new Acceleration(biasX,
23267                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23268         final Acceleration by = new Acceleration(biasY,
23269                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23270         final Acceleration bz = new Acceleration(biasZ,
23271                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23272 
23273         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23274         final double latitude = Math.toRadians(
23275                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23276         final double longitude = Math.toRadians(
23277                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23278         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23279         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23280         final NEDVelocity nedVelocity = new NEDVelocity();
23281         final ECEFPosition ecefPosition = new ECEFPosition();
23282         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23283         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23284                 ecefPosition, ecefVelocity);
23285         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23286                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23287         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23288 
23289         KnownGravityNormAccelerometerCalibrator calibrator =
23290                 new KnownGravityNormAccelerometerCalibrator(
23291                         gravityNorm, measurements,
23292                         true, bx, by, bz);
23293 
23294         // check default values
23295         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23296         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23297         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23298         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23299         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23300         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23301         final Acceleration bx2 = new Acceleration(0.0,
23302                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23303         calibrator.getInitialBiasXAsAcceleration(bx2);
23304         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23305         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23306         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23307         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23308         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23309         final Acceleration by2 = new Acceleration(0.0,
23310                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23311         calibrator.getInitialBiasYAsAcceleration(by2);
23312         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23313         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23314         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23315         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23316         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23317         final Acceleration bz2 = new Acceleration(0.0,
23318                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23319         calibrator.getInitialBiasZAsAcceleration(bz2);
23320         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23321         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23322         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
23323         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
23324         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
23325         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23326         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23327         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23328         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23329         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23330         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23331         final double[] bias1 = calibrator.getInitialBias();
23332         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23333         final double[] bias2 = new double[3];
23334         calibrator.getInitialBias(bias2);
23335         assertArrayEquals(bias1, bias2, 0.0);
23336         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23337         assertEquals(b1, ba);
23338         final Matrix b2 = new Matrix(3, 1);
23339         calibrator.getInitialBiasAsMatrix(b2);
23340         assertEquals(b1, b2);
23341         final Matrix ma1 = calibrator.getInitialMa();
23342         assertEquals(ma1, new Matrix(3, 3));
23343         final Matrix ma2 = new Matrix(3, 3);
23344         calibrator.getInitialMa(ma2);
23345         assertEquals(ma1, ma2);
23346         assertSame(calibrator.getMeasurements(), measurements);
23347         assertTrue(calibrator.isCommonAxisUsed());
23348         assertNull(calibrator.getListener());
23349         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23350         assertFalse(calibrator.isReady());
23351         assertFalse(calibrator.isRunning());
23352         assertNull(calibrator.getEstimatedBiases());
23353         assertFalse(calibrator.getEstimatedBiases(null));
23354         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23355         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23356         assertNull(calibrator.getEstimatedBiasFx());
23357         assertNull(calibrator.getEstimatedBiasFy());
23358         assertNull(calibrator.getEstimatedBiasFz());
23359         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23360         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23361         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23362         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23363         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23364         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23365         assertNull(calibrator.getEstimatedMa());
23366         assertNull(calibrator.getEstimatedSx());
23367         assertNull(calibrator.getEstimatedSy());
23368         assertNull(calibrator.getEstimatedSz());
23369         assertNull(calibrator.getEstimatedMxy());
23370         assertNull(calibrator.getEstimatedMxz());
23371         assertNull(calibrator.getEstimatedMyx());
23372         assertNull(calibrator.getEstimatedMyz());
23373         assertNull(calibrator.getEstimatedMzx());
23374         assertNull(calibrator.getEstimatedMzy());
23375         assertNull(calibrator.getEstimatedCovariance());
23376         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23377         assertNotNull(calibrator.getGroundTruthGravityNorm());
23378         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23379         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23380         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23381                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23382         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23383         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23384         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23385 
23386         // Force IllegalArgumentException
23387         final Acceleration invalidGravityNorm = new Acceleration(
23388                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23389 
23390         calibrator = null;
23391         try {
23392             calibrator = new KnownGravityNormAccelerometerCalibrator(
23393                     invalidGravityNorm, measurements,
23394                     true, bx, by, bz);
23395             fail("IllegalArgumentException expected but not thrown");
23396         } catch (final IllegalArgumentException ignore) {
23397         }
23398         assertNull(calibrator);
23399     }
23400 
23401     @Test
23402     public void testConstructor180() throws WrongSizeException {
23403         final Collection<StandardDeviationBodyKinematics> measurements =
23404                 Collections.emptyList();
23405 
23406         final Matrix ba = generateBa();
23407         final double biasX = ba.getElementAtIndex(0);
23408         final double biasY = ba.getElementAtIndex(1);
23409         final double biasZ = ba.getElementAtIndex(2);
23410 
23411         final Acceleration bx = new Acceleration(biasX,
23412                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23413         final Acceleration by = new Acceleration(biasY,
23414                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23415         final Acceleration bz = new Acceleration(biasZ,
23416                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23417 
23418         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23419         final double latitude = Math.toRadians(
23420                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23421         final double longitude = Math.toRadians(
23422                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23423         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23424         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23425         final NEDVelocity nedVelocity = new NEDVelocity();
23426         final ECEFPosition ecefPosition = new ECEFPosition();
23427         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23428         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23429                 ecefPosition, ecefVelocity);
23430         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23431                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23432         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23433 
23434         KnownGravityNormAccelerometerCalibrator calibrator =
23435                 new KnownGravityNormAccelerometerCalibrator(
23436                         gravityNorm, measurements,
23437                         true, bx, by, bz, this);
23438 
23439         // check default values
23440         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23441         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23442         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23443         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23444         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23445         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23446         final Acceleration bx2 = new Acceleration(0.0,
23447                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23448         calibrator.getInitialBiasXAsAcceleration(bx2);
23449         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23450         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23451         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23452         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23453         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23454         final Acceleration by2 = new Acceleration(0.0,
23455                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23456         calibrator.getInitialBiasYAsAcceleration(by2);
23457         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23458         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23459         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23460         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23461         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23462         final Acceleration bz2 = new Acceleration(0.0,
23463                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23464         calibrator.getInitialBiasZAsAcceleration(bz2);
23465         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23466         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23467         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
23468         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
23469         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
23470         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23471         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23472         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23473         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23474         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23475         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23476         final double[] bias1 = calibrator.getInitialBias();
23477         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23478         final double[] bias2 = new double[3];
23479         calibrator.getInitialBias(bias2);
23480         assertArrayEquals(bias1, bias2, 0.0);
23481         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23482         assertEquals(b1, ba);
23483         final Matrix b2 = new Matrix(3, 1);
23484         calibrator.getInitialBiasAsMatrix(b2);
23485         assertEquals(b1, b2);
23486         final Matrix ma1 = calibrator.getInitialMa();
23487         assertEquals(ma1, new Matrix(3, 3));
23488         final Matrix ma2 = new Matrix(3, 3);
23489         calibrator.getInitialMa(ma2);
23490         assertEquals(ma1, ma2);
23491         assertSame(calibrator.getMeasurements(), measurements);
23492         assertTrue(calibrator.isCommonAxisUsed());
23493         assertSame(calibrator.getListener(), this);
23494         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23495         assertFalse(calibrator.isReady());
23496         assertFalse(calibrator.isRunning());
23497         assertNull(calibrator.getEstimatedBiases());
23498         assertFalse(calibrator.getEstimatedBiases(null));
23499         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23500         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23501         assertNull(calibrator.getEstimatedBiasFx());
23502         assertNull(calibrator.getEstimatedBiasFy());
23503         assertNull(calibrator.getEstimatedBiasFz());
23504         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23505         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23506         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23507         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23508         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23509         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23510         assertNull(calibrator.getEstimatedMa());
23511         assertNull(calibrator.getEstimatedSx());
23512         assertNull(calibrator.getEstimatedSy());
23513         assertNull(calibrator.getEstimatedSz());
23514         assertNull(calibrator.getEstimatedMxy());
23515         assertNull(calibrator.getEstimatedMxz());
23516         assertNull(calibrator.getEstimatedMyx());
23517         assertNull(calibrator.getEstimatedMyz());
23518         assertNull(calibrator.getEstimatedMzx());
23519         assertNull(calibrator.getEstimatedMzy());
23520         assertNull(calibrator.getEstimatedCovariance());
23521         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23522         assertNotNull(calibrator.getGroundTruthGravityNorm());
23523         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23524         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23525         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23526                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23527         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23528         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23529         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23530 
23531         // Force IllegalArgumentException
23532         final Acceleration invalidGravityNorm = new Acceleration(
23533                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23534 
23535         calibrator = null;
23536         try {
23537             calibrator = new KnownGravityNormAccelerometerCalibrator(
23538                     invalidGravityNorm, measurements, true,
23539                     bx, by, bz, this);
23540             fail("IllegalArgumentException expected but not thrown");
23541         } catch (final IllegalArgumentException ignore) {
23542         }
23543         assertNull(calibrator);
23544     }
23545 
23546     @Test
23547     public void testConstructor181() throws WrongSizeException {
23548         final Matrix ba = generateBa();
23549         final double biasX = ba.getElementAtIndex(0);
23550         final double biasY = ba.getElementAtIndex(1);
23551         final double biasZ = ba.getElementAtIndex(2);
23552 
23553         final Matrix ma = generateMaCommonAxis();
23554         final double sx = ma.getElementAt(0, 0);
23555         final double sy = ma.getElementAt(1, 1);
23556         final double sz = ma.getElementAt(2, 2);
23557 
23558         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23559         final double latitude = Math.toRadians(
23560                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23561         final double longitude = Math.toRadians(
23562                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23563         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23564         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23565         final NEDVelocity nedVelocity = new NEDVelocity();
23566         final ECEFPosition ecefPosition = new ECEFPosition();
23567         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23568         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23569                 ecefPosition, ecefVelocity);
23570         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23571                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23572         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23573 
23574         KnownGravityNormAccelerometerCalibrator calibrator =
23575                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
23576                         biasX, biasY, biasZ, sx, sy, sz);
23577 
23578         // check default values
23579         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23580         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23581         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23582         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23583         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23584         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23585         final Acceleration bx2 = new Acceleration(0.0,
23586                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23587         calibrator.getInitialBiasXAsAcceleration(bx2);
23588         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23589         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23590         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23591         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23592         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23593         final Acceleration by2 = new Acceleration(0.0,
23594                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23595         calibrator.getInitialBiasYAsAcceleration(by2);
23596         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23597         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23598         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23599         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23600         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23601         final Acceleration bz2 = new Acceleration(0.0,
23602                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23603         calibrator.getInitialBiasZAsAcceleration(bz2);
23604         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23605         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23606         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23607         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23608         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23609         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23610         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23611         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23612         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23613         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23614         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23615         final double[] bias1 = calibrator.getInitialBias();
23616         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23617         final double[] bias2 = new double[3];
23618         calibrator.getInitialBias(bias2);
23619         assertArrayEquals(bias1, bias2, 0.0);
23620         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23621         assertEquals(b1, ba);
23622         final Matrix b2 = new Matrix(3, 1);
23623         calibrator.getInitialBiasAsMatrix(b2);
23624         assertEquals(b1, b2);
23625         final Matrix ma1 = calibrator.getInitialMa();
23626         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23627         final Matrix ma2 = new Matrix(3, 3);
23628         calibrator.getInitialMa(ma2);
23629         assertEquals(ma1, ma2);
23630         assertNull(calibrator.getMeasurements());
23631         assertFalse(calibrator.isCommonAxisUsed());
23632         assertNull(calibrator.getListener());
23633         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23634         assertFalse(calibrator.isReady());
23635         assertFalse(calibrator.isRunning());
23636         assertNull(calibrator.getEstimatedBiases());
23637         assertFalse(calibrator.getEstimatedBiases(null));
23638         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23639         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23640         assertNull(calibrator.getEstimatedBiasFx());
23641         assertNull(calibrator.getEstimatedBiasFy());
23642         assertNull(calibrator.getEstimatedBiasFz());
23643         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23644         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23645         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23646         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23647         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23648         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23649         assertNull(calibrator.getEstimatedMa());
23650         assertNull(calibrator.getEstimatedSx());
23651         assertNull(calibrator.getEstimatedSy());
23652         assertNull(calibrator.getEstimatedSz());
23653         assertNull(calibrator.getEstimatedMxy());
23654         assertNull(calibrator.getEstimatedMxz());
23655         assertNull(calibrator.getEstimatedMyx());
23656         assertNull(calibrator.getEstimatedMyz());
23657         assertNull(calibrator.getEstimatedMzx());
23658         assertNull(calibrator.getEstimatedMzy());
23659         assertNull(calibrator.getEstimatedCovariance());
23660         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23661         assertNotNull(calibrator.getGroundTruthGravityNorm());
23662         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23663         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23664         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23665                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23666         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23667         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23668         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23669 
23670         // Force IllegalArgumentException
23671         final Acceleration invalidGravityNorm = new Acceleration(
23672                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23673 
23674         calibrator = null;
23675         try {
23676             calibrator = new KnownGravityNormAccelerometerCalibrator(
23677                     invalidGravityNorm, biasX, biasY, biasZ, sx, sy, sz);
23678             fail("IllegalArgumentException expected but not thrown");
23679         } catch (final IllegalArgumentException ignore) {
23680         }
23681         assertNull(calibrator);
23682     }
23683 
23684     @Test
23685     public void testConstructor182() throws WrongSizeException {
23686         final Collection<StandardDeviationBodyKinematics> measurements =
23687                 Collections.emptyList();
23688 
23689         final Matrix ba = generateBa();
23690         final double biasX = ba.getElementAtIndex(0);
23691         final double biasY = ba.getElementAtIndex(1);
23692         final double biasZ = ba.getElementAtIndex(2);
23693 
23694         final Matrix ma = generateMaCommonAxis();
23695         final double sx = ma.getElementAt(0, 0);
23696         final double sy = ma.getElementAt(1, 1);
23697         final double sz = ma.getElementAt(2, 2);
23698 
23699         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23700         final double latitude = Math.toRadians(
23701                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23702         final double longitude = Math.toRadians(
23703                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23704         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23705         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23706         final NEDVelocity nedVelocity = new NEDVelocity();
23707         final ECEFPosition ecefPosition = new ECEFPosition();
23708         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23709         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23710                 ecefPosition, ecefVelocity);
23711         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23712                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23713         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23714 
23715         KnownGravityNormAccelerometerCalibrator calibrator =
23716                 new KnownGravityNormAccelerometerCalibrator(
23717                         gravityNorm, measurements,
23718                         biasX, biasY, biasZ, sx, sy, sz);
23719 
23720         // check default values
23721         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23722         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23723         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23724         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23725         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23726         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23727         final Acceleration bx2 = new Acceleration(0.0,
23728                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23729         calibrator.getInitialBiasXAsAcceleration(bx2);
23730         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23731         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23732         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23733         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23734         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23735         final Acceleration by2 = new Acceleration(0.0,
23736                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23737         calibrator.getInitialBiasYAsAcceleration(by2);
23738         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23739         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23740         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23741         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23742         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23743         final Acceleration bz2 = new Acceleration(0.0,
23744                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23745         calibrator.getInitialBiasZAsAcceleration(bz2);
23746         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23747         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23748         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23749         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23750         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23751         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23752         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23753         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23754         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23755         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23756         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23757         final double[] bias1 = calibrator.getInitialBias();
23758         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23759         final double[] bias2 = new double[3];
23760         calibrator.getInitialBias(bias2);
23761         assertArrayEquals(bias1, bias2, 0.0);
23762         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23763         assertEquals(b1, ba);
23764         final Matrix b2 = new Matrix(3, 1);
23765         calibrator.getInitialBiasAsMatrix(b2);
23766         assertEquals(b1, b2);
23767         final Matrix ma1 = calibrator.getInitialMa();
23768         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23769         final Matrix ma2 = new Matrix(3, 3);
23770         calibrator.getInitialMa(ma2);
23771         assertEquals(ma1, ma2);
23772         assertSame(calibrator.getMeasurements(), measurements);
23773         assertFalse(calibrator.isCommonAxisUsed());
23774         assertNull(calibrator.getListener());
23775         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23776         assertFalse(calibrator.isReady());
23777         assertFalse(calibrator.isRunning());
23778         assertNull(calibrator.getEstimatedBiases());
23779         assertFalse(calibrator.getEstimatedBiases(null));
23780         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23781         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23782         assertNull(calibrator.getEstimatedBiasFx());
23783         assertNull(calibrator.getEstimatedBiasFy());
23784         assertNull(calibrator.getEstimatedBiasFz());
23785         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23786         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23787         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23788         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23789         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23790         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23791         assertNull(calibrator.getEstimatedMa());
23792         assertNull(calibrator.getEstimatedSx());
23793         assertNull(calibrator.getEstimatedSy());
23794         assertNull(calibrator.getEstimatedSz());
23795         assertNull(calibrator.getEstimatedMxy());
23796         assertNull(calibrator.getEstimatedMxz());
23797         assertNull(calibrator.getEstimatedMyx());
23798         assertNull(calibrator.getEstimatedMyz());
23799         assertNull(calibrator.getEstimatedMzx());
23800         assertNull(calibrator.getEstimatedMzy());
23801         assertNull(calibrator.getEstimatedCovariance());
23802         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23803         assertNotNull(calibrator.getGroundTruthGravityNorm());
23804         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23805         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23806         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23807                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23808         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23809         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23810         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23811 
23812         // Force IllegalArgumentException
23813         final Acceleration invalidGravityNorm = new Acceleration(
23814                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23815 
23816         calibrator = null;
23817         try {
23818             calibrator = new KnownGravityNormAccelerometerCalibrator(
23819                     invalidGravityNorm, measurements,
23820                     biasX, biasY, biasZ, sx, sy, sz);
23821             fail("IllegalArgumentException expected but not thrown");
23822         } catch (final IllegalArgumentException ignore) {
23823         }
23824         assertNull(calibrator);
23825     }
23826 
23827     @Test
23828     public void testConstructor183() throws WrongSizeException {
23829         final Collection<StandardDeviationBodyKinematics> measurements =
23830                 Collections.emptyList();
23831 
23832         final Matrix ba = generateBa();
23833         final double biasX = ba.getElementAtIndex(0);
23834         final double biasY = ba.getElementAtIndex(1);
23835         final double biasZ = ba.getElementAtIndex(2);
23836 
23837         final Matrix ma = generateMaCommonAxis();
23838         final double sx = ma.getElementAt(0, 0);
23839         final double sy = ma.getElementAt(1, 1);
23840         final double sz = ma.getElementAt(2, 2);
23841 
23842         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23843         final double latitude = Math.toRadians(
23844                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23845         final double longitude = Math.toRadians(
23846                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23847         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23848         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23849         final NEDVelocity nedVelocity = new NEDVelocity();
23850         final ECEFPosition ecefPosition = new ECEFPosition();
23851         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23852         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23853                 ecefPosition, ecefVelocity);
23854         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23855                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23856         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23857 
23858         KnownGravityNormAccelerometerCalibrator calibrator =
23859                 new KnownGravityNormAccelerometerCalibrator(
23860                         gravityNorm, measurements,
23861                         biasX, biasY, biasZ, sx, sy, sz, this);
23862 
23863         // check default values
23864         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23865         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23866         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23867         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23868         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23869         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23870         final Acceleration bx2 = new Acceleration(0.0,
23871                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23872         calibrator.getInitialBiasXAsAcceleration(bx2);
23873         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23874         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23875         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23876         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23877         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23878         final Acceleration by2 = new Acceleration(0.0,
23879                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23880         calibrator.getInitialBiasYAsAcceleration(by2);
23881         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23882         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23883         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23884         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23885         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23886         final Acceleration bz2 = new Acceleration(0.0,
23887                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23888         calibrator.getInitialBiasZAsAcceleration(bz2);
23889         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23890         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23891         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23892         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23893         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23894         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23895         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23896         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23897         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23898         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23899         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23900         final double[] bias1 = calibrator.getInitialBias();
23901         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23902         final double[] bias2 = new double[3];
23903         calibrator.getInitialBias(bias2);
23904         assertArrayEquals(bias1, bias2, 0.0);
23905         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23906         assertEquals(b1, ba);
23907         final Matrix b2 = new Matrix(3, 1);
23908         calibrator.getInitialBiasAsMatrix(b2);
23909         assertEquals(b1, b2);
23910         final Matrix ma1 = calibrator.getInitialMa();
23911         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23912         final Matrix ma2 = new Matrix(3, 3);
23913         calibrator.getInitialMa(ma2);
23914         assertEquals(ma1, ma2);
23915         assertSame(calibrator.getMeasurements(), measurements);
23916         assertFalse(calibrator.isCommonAxisUsed());
23917         assertSame(calibrator.getListener(), this);
23918         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23919         assertFalse(calibrator.isReady());
23920         assertFalse(calibrator.isRunning());
23921         assertNull(calibrator.getEstimatedBiases());
23922         assertFalse(calibrator.getEstimatedBiases(null));
23923         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23924         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23925         assertNull(calibrator.getEstimatedBiasFx());
23926         assertNull(calibrator.getEstimatedBiasFy());
23927         assertNull(calibrator.getEstimatedBiasFz());
23928         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23929         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23930         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23931         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23932         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23933         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23934         assertNull(calibrator.getEstimatedMa());
23935         assertNull(calibrator.getEstimatedSx());
23936         assertNull(calibrator.getEstimatedSy());
23937         assertNull(calibrator.getEstimatedSz());
23938         assertNull(calibrator.getEstimatedMxy());
23939         assertNull(calibrator.getEstimatedMxz());
23940         assertNull(calibrator.getEstimatedMyx());
23941         assertNull(calibrator.getEstimatedMyz());
23942         assertNull(calibrator.getEstimatedMzx());
23943         assertNull(calibrator.getEstimatedMzy());
23944         assertNull(calibrator.getEstimatedCovariance());
23945         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23946         assertNotNull(calibrator.getGroundTruthGravityNorm());
23947         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23948         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23949         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23950                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23951         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23952         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23953         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23954 
23955         // Force IllegalArgumentException
23956         final Acceleration invalidGravityNorm = new Acceleration(
23957                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23958 
23959         calibrator = null;
23960         try {
23961             calibrator = new KnownGravityNormAccelerometerCalibrator(
23962                     invalidGravityNorm, measurements,
23963                     biasX, biasY, biasZ, sx, sy, sz, this);
23964             fail("IllegalArgumentException expected but not thrown");
23965         } catch (final IllegalArgumentException ignore) {
23966         }
23967         assertNull(calibrator);
23968     }
23969 
23970     @Test
23971     public void testConstructor184() throws WrongSizeException {
23972         final Matrix ba = generateBa();
23973         final double biasX = ba.getElementAtIndex(0);
23974         final double biasY = ba.getElementAtIndex(1);
23975         final double biasZ = ba.getElementAtIndex(2);
23976 
23977         final Matrix ma = generateMaCommonAxis();
23978         final double sx = ma.getElementAt(0, 0);
23979         final double sy = ma.getElementAt(1, 1);
23980         final double sz = ma.getElementAt(2, 2);
23981 
23982         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23983         final double latitude = Math.toRadians(
23984                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23985         final double longitude = Math.toRadians(
23986                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23987         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23988         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23989         final NEDVelocity nedVelocity = new NEDVelocity();
23990         final ECEFPosition ecefPosition = new ECEFPosition();
23991         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23992         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23993                 ecefPosition, ecefVelocity);
23994         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23995                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23996         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
23997 
23998         KnownGravityNormAccelerometerCalibrator calibrator =
23999                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
24000                         true, biasX, biasY, biasZ, sx, sy, sz);
24001 
24002         // check default values
24003         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24004         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24005         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24006         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24007         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24008         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24009         final Acceleration bx2 = new Acceleration(0.0,
24010                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24011         calibrator.getInitialBiasXAsAcceleration(bx2);
24012         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24013         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24014         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24015         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24016         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24017         final Acceleration by2 = new Acceleration(0.0,
24018                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24019         calibrator.getInitialBiasYAsAcceleration(by2);
24020         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24021         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24022         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24023         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24024         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24025         final Acceleration bz2 = new Acceleration(0.0,
24026                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24027         calibrator.getInitialBiasZAsAcceleration(bz2);
24028         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24029         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24030         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24031         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24032         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24033         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24034         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24035         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24036         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24037         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24038         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24039         final double[] bias1 = calibrator.getInitialBias();
24040         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24041         final double[] bias2 = new double[3];
24042         calibrator.getInitialBias(bias2);
24043         assertArrayEquals(bias1, bias2, 0.0);
24044         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24045         assertEquals(b1, ba);
24046         final Matrix b2 = new Matrix(3, 1);
24047         calibrator.getInitialBiasAsMatrix(b2);
24048         assertEquals(b1, b2);
24049         final Matrix ma1 = calibrator.getInitialMa();
24050         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24051         final Matrix ma2 = new Matrix(3, 3);
24052         calibrator.getInitialMa(ma2);
24053         assertEquals(ma1, ma2);
24054         assertNull(calibrator.getMeasurements());
24055         assertTrue(calibrator.isCommonAxisUsed());
24056         assertNull(calibrator.getListener());
24057         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24058         assertFalse(calibrator.isReady());
24059         assertFalse(calibrator.isRunning());
24060         assertNull(calibrator.getEstimatedBiases());
24061         assertFalse(calibrator.getEstimatedBiases(null));
24062         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24063         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24064         assertNull(calibrator.getEstimatedBiasFx());
24065         assertNull(calibrator.getEstimatedBiasFy());
24066         assertNull(calibrator.getEstimatedBiasFz());
24067         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24068         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24069         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24070         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24071         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24072         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24073         assertNull(calibrator.getEstimatedMa());
24074         assertNull(calibrator.getEstimatedSx());
24075         assertNull(calibrator.getEstimatedSy());
24076         assertNull(calibrator.getEstimatedSz());
24077         assertNull(calibrator.getEstimatedMxy());
24078         assertNull(calibrator.getEstimatedMxz());
24079         assertNull(calibrator.getEstimatedMyx());
24080         assertNull(calibrator.getEstimatedMyz());
24081         assertNull(calibrator.getEstimatedMzx());
24082         assertNull(calibrator.getEstimatedMzy());
24083         assertNull(calibrator.getEstimatedCovariance());
24084         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24085         assertNotNull(calibrator.getGroundTruthGravityNorm());
24086         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24087         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24088         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24089                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24090         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24091         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24092         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24093 
24094         // Force IllegalArgumentException
24095         final Acceleration invalidGravityNorm = new Acceleration(
24096                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24097 
24098         calibrator = null;
24099         try {
24100             calibrator = new KnownGravityNormAccelerometerCalibrator(
24101                     invalidGravityNorm, true, biasX, biasY, biasZ,
24102                     sx, sy, sz);
24103             fail("IllegalArgumentException expected but not thrown");
24104         } catch (final IllegalArgumentException ignore) {
24105         }
24106         assertNull(calibrator);
24107     }
24108 
24109     @Test
24110     public void testConstructor185() throws WrongSizeException {
24111         final Matrix ba = generateBa();
24112         final double biasX = ba.getElementAtIndex(0);
24113         final double biasY = ba.getElementAtIndex(1);
24114         final double biasZ = ba.getElementAtIndex(2);
24115 
24116         final Matrix ma = generateMaCommonAxis();
24117         final double sx = ma.getElementAt(0, 0);
24118         final double sy = ma.getElementAt(1, 1);
24119         final double sz = ma.getElementAt(2, 2);
24120 
24121         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24122         final double latitude = Math.toRadians(
24123                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24124         final double longitude = Math.toRadians(
24125                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24126         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24127         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24128         final NEDVelocity nedVelocity = new NEDVelocity();
24129         final ECEFPosition ecefPosition = new ECEFPosition();
24130         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24131         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24132                 ecefPosition, ecefVelocity);
24133         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24134                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24135         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24136 
24137         KnownGravityNormAccelerometerCalibrator calibrator =
24138                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
24139                         true, biasX, biasY, biasZ, sx, sy, sz,
24140                         this);
24141 
24142         // check default values
24143         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24144         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24145         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24146         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24147         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24148         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24149         final Acceleration bx2 = new Acceleration(0.0,
24150                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24151         calibrator.getInitialBiasXAsAcceleration(bx2);
24152         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24153         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24154         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24155         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24156         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24157         final Acceleration by2 = new Acceleration(0.0,
24158                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24159         calibrator.getInitialBiasYAsAcceleration(by2);
24160         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24161         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24162         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24163         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24164         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24165         final Acceleration bz2 = new Acceleration(0.0,
24166                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24167         calibrator.getInitialBiasZAsAcceleration(bz2);
24168         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24169         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24170         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24171         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24172         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24173         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24174         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24175         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24176         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24177         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24178         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24179         final double[] bias1 = calibrator.getInitialBias();
24180         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24181         final double[] bias2 = new double[3];
24182         calibrator.getInitialBias(bias2);
24183         assertArrayEquals(bias1, bias2, 0.0);
24184         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24185         assertEquals(b1, ba);
24186         final Matrix b2 = new Matrix(3, 1);
24187         calibrator.getInitialBiasAsMatrix(b2);
24188         assertEquals(b1, b2);
24189         final Matrix ma1 = calibrator.getInitialMa();
24190         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24191         final Matrix ma2 = new Matrix(3, 3);
24192         calibrator.getInitialMa(ma2);
24193         assertEquals(ma1, ma2);
24194         assertNull(calibrator.getMeasurements());
24195         assertTrue(calibrator.isCommonAxisUsed());
24196         assertSame(calibrator.getListener(), this);
24197         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24198         assertFalse(calibrator.isReady());
24199         assertFalse(calibrator.isRunning());
24200         assertNull(calibrator.getEstimatedBiases());
24201         assertFalse(calibrator.getEstimatedBiases(null));
24202         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24203         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24204         assertNull(calibrator.getEstimatedBiasFx());
24205         assertNull(calibrator.getEstimatedBiasFy());
24206         assertNull(calibrator.getEstimatedBiasFz());
24207         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24208         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24209         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24210         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24211         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24212         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24213         assertNull(calibrator.getEstimatedMa());
24214         assertNull(calibrator.getEstimatedSx());
24215         assertNull(calibrator.getEstimatedSy());
24216         assertNull(calibrator.getEstimatedSz());
24217         assertNull(calibrator.getEstimatedMxy());
24218         assertNull(calibrator.getEstimatedMxz());
24219         assertNull(calibrator.getEstimatedMyx());
24220         assertNull(calibrator.getEstimatedMyz());
24221         assertNull(calibrator.getEstimatedMzx());
24222         assertNull(calibrator.getEstimatedMzy());
24223         assertNull(calibrator.getEstimatedCovariance());
24224         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24225         assertNotNull(calibrator.getGroundTruthGravityNorm());
24226         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24227         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24228         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24229                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24230         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24231         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24232         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24233 
24234         // Force IllegalArgumentException
24235         final Acceleration invalidGravityNorm = new Acceleration(
24236                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24237 
24238         calibrator = null;
24239         try {
24240             calibrator = new KnownGravityNormAccelerometerCalibrator(
24241                     invalidGravityNorm, true,
24242                     biasX, biasY, biasZ,
24243                     sx, sy, sz, this);
24244             fail("IllegalArgumentException expected but not thrown");
24245         } catch (final IllegalArgumentException ignore) {
24246         }
24247         assertNull(calibrator);
24248     }
24249 
24250     @Test
24251     public void testConstructor186() throws WrongSizeException {
24252         final Collection<StandardDeviationBodyKinematics> measurements =
24253                 Collections.emptyList();
24254 
24255         final Matrix ba = generateBa();
24256         final double biasX = ba.getElementAtIndex(0);
24257         final double biasY = ba.getElementAtIndex(1);
24258         final double biasZ = ba.getElementAtIndex(2);
24259 
24260         final Matrix ma = generateMaCommonAxis();
24261         final double sx = ma.getElementAt(0, 0);
24262         final double sy = ma.getElementAt(1, 1);
24263         final double sz = ma.getElementAt(2, 2);
24264 
24265         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24266         final double latitude = Math.toRadians(
24267                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24268         final double longitude = Math.toRadians(
24269                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24270         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24271         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24272         final NEDVelocity nedVelocity = new NEDVelocity();
24273         final ECEFPosition ecefPosition = new ECEFPosition();
24274         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24275         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24276                 ecefPosition, ecefVelocity);
24277         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24278                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24279         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24280 
24281         KnownGravityNormAccelerometerCalibrator calibrator =
24282                 new KnownGravityNormAccelerometerCalibrator(
24283                         gravityNorm, measurements,
24284                         true, biasX, biasY, biasZ, sx, sy, sz);
24285 
24286         // check default values
24287         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24288         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24289         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24290         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24291         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24292         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24293         final Acceleration bx2 = new Acceleration(0.0,
24294                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24295         calibrator.getInitialBiasXAsAcceleration(bx2);
24296         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24297         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24298         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24299         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24300         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24301         final Acceleration by2 = new Acceleration(0.0,
24302                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24303         calibrator.getInitialBiasYAsAcceleration(by2);
24304         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24305         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24306         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24307         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24308         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24309         final Acceleration bz2 = new Acceleration(0.0,
24310                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24311         calibrator.getInitialBiasZAsAcceleration(bz2);
24312         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24313         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24314         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24315         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24316         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24317         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24318         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24319         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24320         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24321         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24322         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24323         final double[] bias1 = calibrator.getInitialBias();
24324         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24325         final double[] bias2 = new double[3];
24326         calibrator.getInitialBias(bias2);
24327         assertArrayEquals(bias1, bias2, 0.0);
24328         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24329         assertEquals(b1, ba);
24330         final Matrix b2 = new Matrix(3, 1);
24331         calibrator.getInitialBiasAsMatrix(b2);
24332         assertEquals(b1, b2);
24333         final Matrix ma1 = calibrator.getInitialMa();
24334         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24335         final Matrix ma2 = new Matrix(3, 3);
24336         calibrator.getInitialMa(ma2);
24337         assertEquals(ma1, ma2);
24338         assertSame(calibrator.getMeasurements(), measurements);
24339         assertTrue(calibrator.isCommonAxisUsed());
24340         assertNull(calibrator.getListener());
24341         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24342         assertFalse(calibrator.isReady());
24343         assertFalse(calibrator.isRunning());
24344         assertNull(calibrator.getEstimatedBiases());
24345         assertFalse(calibrator.getEstimatedBiases(null));
24346         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24347         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24348         assertNull(calibrator.getEstimatedBiasFx());
24349         assertNull(calibrator.getEstimatedBiasFy());
24350         assertNull(calibrator.getEstimatedBiasFz());
24351         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24352         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24353         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24354         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24355         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24356         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24357         assertNull(calibrator.getEstimatedMa());
24358         assertNull(calibrator.getEstimatedSx());
24359         assertNull(calibrator.getEstimatedSy());
24360         assertNull(calibrator.getEstimatedSz());
24361         assertNull(calibrator.getEstimatedMxy());
24362         assertNull(calibrator.getEstimatedMxz());
24363         assertNull(calibrator.getEstimatedMyx());
24364         assertNull(calibrator.getEstimatedMyz());
24365         assertNull(calibrator.getEstimatedMzx());
24366         assertNull(calibrator.getEstimatedMzy());
24367         assertNull(calibrator.getEstimatedCovariance());
24368         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24369         assertNotNull(calibrator.getGroundTruthGravityNorm());
24370         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24371         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24372         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24373                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24374         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24375         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24376         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24377 
24378         // Force IllegalArgumentException
24379         final Acceleration invalidGravityNorm = new Acceleration(
24380                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24381 
24382         calibrator = null;
24383         try {
24384             calibrator = new KnownGravityNormAccelerometerCalibrator(
24385                     invalidGravityNorm, measurements,
24386                     true, biasX, biasY, biasZ, sx, sy, sz);
24387             fail("IllegalArgumentException expected but not thrown");
24388         } catch (final IllegalArgumentException ignore) {
24389         }
24390         assertNull(calibrator);
24391     }
24392 
24393     @Test
24394     public void testConstructor187() throws WrongSizeException {
24395         final Collection<StandardDeviationBodyKinematics> measurements =
24396                 Collections.emptyList();
24397 
24398         final Matrix ba = generateBa();
24399         final double biasX = ba.getElementAtIndex(0);
24400         final double biasY = ba.getElementAtIndex(1);
24401         final double biasZ = ba.getElementAtIndex(2);
24402 
24403         final Matrix ma = generateMaCommonAxis();
24404         final double sx = ma.getElementAt(0, 0);
24405         final double sy = ma.getElementAt(1, 1);
24406         final double sz = ma.getElementAt(2, 2);
24407 
24408         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24409         final double latitude = Math.toRadians(
24410                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24411         final double longitude = Math.toRadians(
24412                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24413         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24414         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24415         final NEDVelocity nedVelocity = new NEDVelocity();
24416         final ECEFPosition ecefPosition = new ECEFPosition();
24417         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24418         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24419                 ecefPosition, ecefVelocity);
24420         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24421                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24422         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24423 
24424         KnownGravityNormAccelerometerCalibrator calibrator =
24425                 new KnownGravityNormAccelerometerCalibrator(
24426                         gravityNorm, measurements,
24427                         true, biasX, biasY, biasZ, sx, sy, sz,
24428                         this);
24429 
24430         // check default values
24431         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24432         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24433         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24434         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24435         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24436         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24437         final Acceleration bx2 = new Acceleration(0.0,
24438                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24439         calibrator.getInitialBiasXAsAcceleration(bx2);
24440         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24441         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24442         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24443         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24444         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24445         final Acceleration by2 = new Acceleration(0.0,
24446                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24447         calibrator.getInitialBiasYAsAcceleration(by2);
24448         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24449         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24450         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24451         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24452         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24453         final Acceleration bz2 = new Acceleration(0.0,
24454                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24455         calibrator.getInitialBiasZAsAcceleration(bz2);
24456         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24457         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24458         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24459         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24460         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24461         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24462         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24463         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24464         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24465         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24466         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24467         final double[] bias1 = calibrator.getInitialBias();
24468         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24469         final double[] bias2 = new double[3];
24470         calibrator.getInitialBias(bias2);
24471         assertArrayEquals(bias1, bias2, 0.0);
24472         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24473         assertEquals(b1, ba);
24474         final Matrix b2 = new Matrix(3, 1);
24475         calibrator.getInitialBiasAsMatrix(b2);
24476         assertEquals(b1, b2);
24477         final Matrix ma1 = calibrator.getInitialMa();
24478         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24479         final Matrix ma2 = new Matrix(3, 3);
24480         calibrator.getInitialMa(ma2);
24481         assertEquals(ma1, ma2);
24482         assertSame(calibrator.getMeasurements(), measurements);
24483         assertTrue(calibrator.isCommonAxisUsed());
24484         assertSame(calibrator.getListener(), this);
24485         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24486         assertFalse(calibrator.isReady());
24487         assertFalse(calibrator.isRunning());
24488         assertNull(calibrator.getEstimatedBiases());
24489         assertFalse(calibrator.getEstimatedBiases(null));
24490         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24491         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24492         assertNull(calibrator.getEstimatedBiasFx());
24493         assertNull(calibrator.getEstimatedBiasFy());
24494         assertNull(calibrator.getEstimatedBiasFz());
24495         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24496         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24497         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24498         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24499         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24500         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24501         assertNull(calibrator.getEstimatedMa());
24502         assertNull(calibrator.getEstimatedSx());
24503         assertNull(calibrator.getEstimatedSy());
24504         assertNull(calibrator.getEstimatedSz());
24505         assertNull(calibrator.getEstimatedMxy());
24506         assertNull(calibrator.getEstimatedMxz());
24507         assertNull(calibrator.getEstimatedMyx());
24508         assertNull(calibrator.getEstimatedMyz());
24509         assertNull(calibrator.getEstimatedMzx());
24510         assertNull(calibrator.getEstimatedMzy());
24511         assertNull(calibrator.getEstimatedCovariance());
24512         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24513         assertNotNull(calibrator.getGroundTruthGravityNorm());
24514         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24515         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24516         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24517                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24518         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24519         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24520         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24521 
24522         // Force IllegalArgumentException
24523         final Acceleration invalidGravityNorm = new Acceleration(
24524                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24525 
24526         calibrator = null;
24527         try {
24528             calibrator = new KnownGravityNormAccelerometerCalibrator(
24529                     invalidGravityNorm, measurements,
24530                     true, biasX, biasY, biasZ, sx, sy, sz,
24531                     this);
24532             fail("IllegalArgumentException expected but not thrown");
24533         } catch (final IllegalArgumentException ignore) {
24534         }
24535         assertNull(calibrator);
24536     }
24537 
24538     @Test
24539     public void testConstructor188() throws WrongSizeException {
24540         final Matrix ba = generateBa();
24541         final double biasX = ba.getElementAtIndex(0);
24542         final double biasY = ba.getElementAtIndex(1);
24543         final double biasZ = ba.getElementAtIndex(2);
24544 
24545         final Acceleration bx = new Acceleration(biasX,
24546                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24547         final Acceleration by = new Acceleration(biasY,
24548                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24549         final Acceleration bz = new Acceleration(biasZ,
24550                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24551 
24552         final Matrix ma = generateMaCommonAxis();
24553         final double sx = ma.getElementAt(0, 0);
24554         final double sy = ma.getElementAt(1, 1);
24555         final double sz = ma.getElementAt(2, 2);
24556 
24557         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24558         final double latitude = Math.toRadians(
24559                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24560         final double longitude = Math.toRadians(
24561                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24562         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24563         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24564         final NEDVelocity nedVelocity = new NEDVelocity();
24565         final ECEFPosition ecefPosition = new ECEFPosition();
24566         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24567         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24568                 ecefPosition, ecefVelocity);
24569         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24570                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24571         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24572 
24573         KnownGravityNormAccelerometerCalibrator calibrator =
24574                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
24575                         bx, by, bz, sx, sy, sz);
24576 
24577         // check default values
24578         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24579         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24580         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24581         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24582         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24583         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24584         final Acceleration bx2 = new Acceleration(0.0,
24585                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24586         calibrator.getInitialBiasXAsAcceleration(bx2);
24587         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24588         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24589         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24590         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24591         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24592         final Acceleration by2 = new Acceleration(0.0,
24593                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24594         calibrator.getInitialBiasYAsAcceleration(by2);
24595         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24596         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24597         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24598         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24599         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24600         final Acceleration bz2 = new Acceleration(0.0,
24601                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24602         calibrator.getInitialBiasZAsAcceleration(bz2);
24603         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24604         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24605         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24606         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24607         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24608         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24609         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24610         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24611         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24612         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24613         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24614         final double[] bias1 = calibrator.getInitialBias();
24615         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24616         final double[] bias2 = new double[3];
24617         calibrator.getInitialBias(bias2);
24618         assertArrayEquals(bias1, bias2, 0.0);
24619         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24620         assertEquals(b1, ba);
24621         final Matrix b2 = new Matrix(3, 1);
24622         calibrator.getInitialBiasAsMatrix(b2);
24623         assertEquals(b1, b2);
24624         final Matrix ma1 = calibrator.getInitialMa();
24625         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24626         final Matrix ma2 = new Matrix(3, 3);
24627         calibrator.getInitialMa(ma2);
24628         assertEquals(ma1, ma2);
24629         assertNull(calibrator.getMeasurements());
24630         assertFalse(calibrator.isCommonAxisUsed());
24631         assertNull(calibrator.getListener());
24632         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24633         assertFalse(calibrator.isReady());
24634         assertFalse(calibrator.isRunning());
24635         assertNull(calibrator.getEstimatedBiases());
24636         assertFalse(calibrator.getEstimatedBiases(null));
24637         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24638         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24639         assertNull(calibrator.getEstimatedBiasFx());
24640         assertNull(calibrator.getEstimatedBiasFy());
24641         assertNull(calibrator.getEstimatedBiasFz());
24642         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24643         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24644         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24645         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24646         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24647         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24648         assertNull(calibrator.getEstimatedMa());
24649         assertNull(calibrator.getEstimatedSx());
24650         assertNull(calibrator.getEstimatedSy());
24651         assertNull(calibrator.getEstimatedSz());
24652         assertNull(calibrator.getEstimatedMxy());
24653         assertNull(calibrator.getEstimatedMxz());
24654         assertNull(calibrator.getEstimatedMyx());
24655         assertNull(calibrator.getEstimatedMyz());
24656         assertNull(calibrator.getEstimatedMzx());
24657         assertNull(calibrator.getEstimatedMzy());
24658         assertNull(calibrator.getEstimatedCovariance());
24659         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24660         assertNotNull(calibrator.getGroundTruthGravityNorm());
24661         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24662         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24663         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24664                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24665         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24666         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24667         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24668 
24669         // Force IllegalArgumentException
24670         final Acceleration invalidGravityNorm = new Acceleration(
24671                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24672 
24673         calibrator = null;
24674         try {
24675             calibrator = new KnownGravityNormAccelerometerCalibrator(
24676                     invalidGravityNorm,
24677                     bx, by, bz, sx, sy, sz);
24678             fail("IllegalArgumentException expected but not thrown");
24679         } catch (final IllegalArgumentException ignore) {
24680         }
24681         assertNull(calibrator);
24682     }
24683 
24684     @Test
24685     public void testConstructor189() throws WrongSizeException {
24686         final Matrix ba = generateBa();
24687         final double biasX = ba.getElementAtIndex(0);
24688         final double biasY = ba.getElementAtIndex(1);
24689         final double biasZ = ba.getElementAtIndex(2);
24690 
24691         final Acceleration bx = new Acceleration(biasX,
24692                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24693         final Acceleration by = new Acceleration(biasY,
24694                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24695         final Acceleration bz = new Acceleration(biasZ,
24696                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24697 
24698         final Matrix ma = generateMaCommonAxis();
24699         final double sx = ma.getElementAt(0, 0);
24700         final double sy = ma.getElementAt(1, 1);
24701         final double sz = ma.getElementAt(2, 2);
24702 
24703         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24704         final double latitude = Math.toRadians(
24705                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24706         final double longitude = Math.toRadians(
24707                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24708         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24709         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24710         final NEDVelocity nedVelocity = new NEDVelocity();
24711         final ECEFPosition ecefPosition = new ECEFPosition();
24712         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24713         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24714                 ecefPosition, ecefVelocity);
24715         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24716                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24717         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24718 
24719         KnownGravityNormAccelerometerCalibrator calibrator =
24720                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
24721                         bx, by, bz, sx, sy, sz, this);
24722 
24723         // check default values
24724         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24725         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24726         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24727         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24728         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24729         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24730         final Acceleration bx2 = new Acceleration(0.0,
24731                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24732         calibrator.getInitialBiasXAsAcceleration(bx2);
24733         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24734         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24735         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24736         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24737         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24738         final Acceleration by2 = new Acceleration(0.0,
24739                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24740         calibrator.getInitialBiasYAsAcceleration(by2);
24741         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24742         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24743         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24744         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24745         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24746         final Acceleration bz2 = new Acceleration(0.0,
24747                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24748         calibrator.getInitialBiasZAsAcceleration(bz2);
24749         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24750         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24751         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24752         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24753         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24754         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24755         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24756         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24757         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24758         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24759         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24760         final double[] bias1 = calibrator.getInitialBias();
24761         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24762         final double[] bias2 = new double[3];
24763         calibrator.getInitialBias(bias2);
24764         assertArrayEquals(bias1, bias2, 0.0);
24765         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24766         assertEquals(b1, ba);
24767         final Matrix b2 = new Matrix(3, 1);
24768         calibrator.getInitialBiasAsMatrix(b2);
24769         assertEquals(b1, b2);
24770         final Matrix ma1 = calibrator.getInitialMa();
24771         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24772         final Matrix ma2 = new Matrix(3, 3);
24773         calibrator.getInitialMa(ma2);
24774         assertEquals(ma1, ma2);
24775         assertNull(calibrator.getMeasurements());
24776         assertFalse(calibrator.isCommonAxisUsed());
24777         assertSame(calibrator.getListener(), this);
24778         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24779         assertFalse(calibrator.isReady());
24780         assertFalse(calibrator.isRunning());
24781         assertNull(calibrator.getEstimatedBiases());
24782         assertFalse(calibrator.getEstimatedBiases(null));
24783         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24784         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24785         assertNull(calibrator.getEstimatedBiasFx());
24786         assertNull(calibrator.getEstimatedBiasFy());
24787         assertNull(calibrator.getEstimatedBiasFz());
24788         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24789         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24790         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24791         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24792         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24793         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24794         assertNull(calibrator.getEstimatedMa());
24795         assertNull(calibrator.getEstimatedSx());
24796         assertNull(calibrator.getEstimatedSy());
24797         assertNull(calibrator.getEstimatedSz());
24798         assertNull(calibrator.getEstimatedMxy());
24799         assertNull(calibrator.getEstimatedMxz());
24800         assertNull(calibrator.getEstimatedMyx());
24801         assertNull(calibrator.getEstimatedMyz());
24802         assertNull(calibrator.getEstimatedMzx());
24803         assertNull(calibrator.getEstimatedMzy());
24804         assertNull(calibrator.getEstimatedCovariance());
24805         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24806         assertNotNull(calibrator.getGroundTruthGravityNorm());
24807         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24808         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24809         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24810                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24811         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24812         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24813         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24814 
24815         // Force IllegalArgumentException
24816         final Acceleration invalidGravityNorm = new Acceleration(
24817                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24818 
24819         calibrator = null;
24820         try {
24821             calibrator = new KnownGravityNormAccelerometerCalibrator(
24822                     invalidGravityNorm, bx, by, bz, sx, sy, sz,
24823                     this);
24824             fail("IllegalArgumentException expected but not thrown");
24825         } catch (final IllegalArgumentException ignore) {
24826         }
24827         assertNull(calibrator);
24828     }
24829 
24830     @Test
24831     public void testConstructor190() throws WrongSizeException {
24832         final Collection<StandardDeviationBodyKinematics> measurements =
24833                 Collections.emptyList();
24834 
24835         final Matrix ba = generateBa();
24836         final double biasX = ba.getElementAtIndex(0);
24837         final double biasY = ba.getElementAtIndex(1);
24838         final double biasZ = ba.getElementAtIndex(2);
24839 
24840         final Acceleration bx = new Acceleration(biasX,
24841                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24842         final Acceleration by = new Acceleration(biasY,
24843                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24844         final Acceleration bz = new Acceleration(biasZ,
24845                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24846 
24847         final Matrix ma = generateMaCommonAxis();
24848         final double sx = ma.getElementAt(0, 0);
24849         final double sy = ma.getElementAt(1, 1);
24850         final double sz = ma.getElementAt(2, 2);
24851 
24852         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24853         final double latitude = Math.toRadians(
24854                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24855         final double longitude = Math.toRadians(
24856                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24857         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24858         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24859         final NEDVelocity nedVelocity = new NEDVelocity();
24860         final ECEFPosition ecefPosition = new ECEFPosition();
24861         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24862         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24863                 ecefPosition, ecefVelocity);
24864         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24865                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24866         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
24867 
24868         KnownGravityNormAccelerometerCalibrator calibrator =
24869                 new KnownGravityNormAccelerometerCalibrator(
24870                         gravityNorm, measurements,
24871                         bx, by, bz, sx, sy, sz);
24872 
24873         // check default values
24874         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24875         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24876         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24877         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24878         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24879         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24880         final Acceleration bx2 = new Acceleration(0.0,
24881                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24882         calibrator.getInitialBiasXAsAcceleration(bx2);
24883         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24884         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24885         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24886         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24887         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24888         final Acceleration by2 = new Acceleration(0.0,
24889                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24890         calibrator.getInitialBiasYAsAcceleration(by2);
24891         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24892         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24893         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24894         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24895         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24896         final Acceleration bz2 = new Acceleration(0.0,
24897                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24898         calibrator.getInitialBiasZAsAcceleration(bz2);
24899         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24900         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24901         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24902         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24903         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24904         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24905         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24906         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24907         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24908         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24909         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24910         final double[] bias1 = calibrator.getInitialBias();
24911         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24912         final double[] bias2 = new double[3];
24913         calibrator.getInitialBias(bias2);
24914         assertArrayEquals(bias1, bias2, 0.0);
24915         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24916         assertEquals(b1, ba);
24917         final Matrix b2 = new Matrix(3, 1);
24918         calibrator.getInitialBiasAsMatrix(b2);
24919         assertEquals(b1, b2);
24920         final Matrix ma1 = calibrator.getInitialMa();
24921         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24922         final Matrix ma2 = new Matrix(3, 3);
24923         calibrator.getInitialMa(ma2);
24924         assertEquals(ma1, ma2);
24925         assertSame(calibrator.getMeasurements(), measurements);
24926         assertFalse(calibrator.isCommonAxisUsed());
24927         assertNull(calibrator.getListener());
24928         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24929         assertFalse(calibrator.isReady());
24930         assertFalse(calibrator.isRunning());
24931         assertNull(calibrator.getEstimatedBiases());
24932         assertFalse(calibrator.getEstimatedBiases(null));
24933         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24934         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24935         assertNull(calibrator.getEstimatedBiasFx());
24936         assertNull(calibrator.getEstimatedBiasFy());
24937         assertNull(calibrator.getEstimatedBiasFz());
24938         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24939         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24940         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24941         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24942         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24943         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24944         assertNull(calibrator.getEstimatedMa());
24945         assertNull(calibrator.getEstimatedSx());
24946         assertNull(calibrator.getEstimatedSy());
24947         assertNull(calibrator.getEstimatedSz());
24948         assertNull(calibrator.getEstimatedMxy());
24949         assertNull(calibrator.getEstimatedMxz());
24950         assertNull(calibrator.getEstimatedMyx());
24951         assertNull(calibrator.getEstimatedMyz());
24952         assertNull(calibrator.getEstimatedMzx());
24953         assertNull(calibrator.getEstimatedMzy());
24954         assertNull(calibrator.getEstimatedCovariance());
24955         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24956         assertNotNull(calibrator.getGroundTruthGravityNorm());
24957         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24958         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24959         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24960                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24961         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24962         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24963         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24964 
24965         // Force IllegalArgumentException
24966         final Acceleration invalidGravityNorm = new Acceleration(
24967                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24968 
24969         calibrator = null;
24970         try {
24971             calibrator = new KnownGravityNormAccelerometerCalibrator(
24972                     invalidGravityNorm, measurements,
24973                     bx, by, bz, sx, sy, sz);
24974             fail("IllegalArgumentException expected but not thrown");
24975         } catch (final IllegalArgumentException ignore) {
24976         }
24977         assertNull(calibrator);
24978     }
24979 
24980     @Test
24981     public void testConstructor191() throws WrongSizeException {
24982         final Collection<StandardDeviationBodyKinematics> measurements =
24983                 Collections.emptyList();
24984 
24985         final Matrix ba = generateBa();
24986         final double biasX = ba.getElementAtIndex(0);
24987         final double biasY = ba.getElementAtIndex(1);
24988         final double biasZ = ba.getElementAtIndex(2);
24989 
24990         final Acceleration bx = new Acceleration(biasX,
24991                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24992         final Acceleration by = new Acceleration(biasY,
24993                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24994         final Acceleration bz = new Acceleration(biasZ,
24995                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24996 
24997         final Matrix ma = generateMaCommonAxis();
24998         final double sx = ma.getElementAt(0, 0);
24999         final double sy = ma.getElementAt(1, 1);
25000         final double sz = ma.getElementAt(2, 2);
25001 
25002         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25003         final double latitude = Math.toRadians(
25004                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25005         final double longitude = Math.toRadians(
25006                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25007         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25008         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25009         final NEDVelocity nedVelocity = new NEDVelocity();
25010         final ECEFPosition ecefPosition = new ECEFPosition();
25011         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25012         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25013                 ecefPosition, ecefVelocity);
25014         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25015                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25016         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25017 
25018         KnownGravityNormAccelerometerCalibrator calibrator =
25019                 new KnownGravityNormAccelerometerCalibrator(
25020                         gravityNorm, measurements,
25021                         bx, by, bz, sx, sy, sz, this);
25022 
25023         // check default values
25024         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25025         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25026         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25027         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25028         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25029         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25030         final Acceleration bx2 = new Acceleration(0.0,
25031                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25032         calibrator.getInitialBiasXAsAcceleration(bx2);
25033         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25034         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25035         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25036         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25037         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25038         final Acceleration by2 = new Acceleration(0.0,
25039                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25040         calibrator.getInitialBiasYAsAcceleration(by2);
25041         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25042         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25043         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25044         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25045         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25046         final Acceleration bz2 = new Acceleration(0.0,
25047                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25048         calibrator.getInitialBiasZAsAcceleration(bz2);
25049         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25050         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25051         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25052         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25053         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25054         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25055         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25056         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25057         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25058         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25059         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25060         final double[] bias1 = calibrator.getInitialBias();
25061         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25062         final double[] bias2 = new double[3];
25063         calibrator.getInitialBias(bias2);
25064         assertArrayEquals(bias1, bias2, 0.0);
25065         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25066         assertEquals(b1, ba);
25067         final Matrix b2 = new Matrix(3, 1);
25068         calibrator.getInitialBiasAsMatrix(b2);
25069         assertEquals(b1, b2);
25070         final Matrix ma1 = calibrator.getInitialMa();
25071         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
25072         final Matrix ma2 = new Matrix(3, 3);
25073         calibrator.getInitialMa(ma2);
25074         assertEquals(ma1, ma2);
25075         assertSame(calibrator.getMeasurements(), measurements);
25076         assertFalse(calibrator.isCommonAxisUsed());
25077         assertSame(calibrator.getListener(), this);
25078         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25079         assertFalse(calibrator.isReady());
25080         assertFalse(calibrator.isRunning());
25081         assertNull(calibrator.getEstimatedBiases());
25082         assertFalse(calibrator.getEstimatedBiases(null));
25083         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25084         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25085         assertNull(calibrator.getEstimatedBiasFx());
25086         assertNull(calibrator.getEstimatedBiasFy());
25087         assertNull(calibrator.getEstimatedBiasFz());
25088         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25089         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25090         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25091         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25092         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25093         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25094         assertNull(calibrator.getEstimatedMa());
25095         assertNull(calibrator.getEstimatedSx());
25096         assertNull(calibrator.getEstimatedSy());
25097         assertNull(calibrator.getEstimatedSz());
25098         assertNull(calibrator.getEstimatedMxy());
25099         assertNull(calibrator.getEstimatedMxz());
25100         assertNull(calibrator.getEstimatedMyx());
25101         assertNull(calibrator.getEstimatedMyz());
25102         assertNull(calibrator.getEstimatedMzx());
25103         assertNull(calibrator.getEstimatedMzy());
25104         assertNull(calibrator.getEstimatedCovariance());
25105         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25106         assertNotNull(calibrator.getGroundTruthGravityNorm());
25107         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25108         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25109         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25110                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25111         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25112         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25113         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25114 
25115         // Force IllegalArgumentException
25116         final Acceleration invalidGravityNorm = new Acceleration(
25117                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25118 
25119         calibrator = null;
25120         try {
25121             calibrator = new KnownGravityNormAccelerometerCalibrator(
25122                     invalidGravityNorm, measurements,
25123                     bx, by, bz, sx, sy, sz, this);
25124             fail("IllegalArgumentException expected but not thrown");
25125         } catch (final IllegalArgumentException ignore) {
25126         }
25127         assertNull(calibrator);
25128     }
25129 
25130     @Test
25131     public void testConstructor192() throws WrongSizeException {
25132         final Matrix ba = generateBa();
25133         final double biasX = ba.getElementAtIndex(0);
25134         final double biasY = ba.getElementAtIndex(1);
25135         final double biasZ = ba.getElementAtIndex(2);
25136 
25137         final Acceleration bx = new Acceleration(biasX,
25138                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25139         final Acceleration by = new Acceleration(biasY,
25140                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25141         final Acceleration bz = new Acceleration(biasZ,
25142                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25143 
25144         final Matrix ma = generateMaCommonAxis();
25145         final double sx = ma.getElementAt(0, 0);
25146         final double sy = ma.getElementAt(1, 1);
25147         final double sz = ma.getElementAt(2, 2);
25148 
25149         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25150         final double latitude = Math.toRadians(
25151                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25152         final double longitude = Math.toRadians(
25153                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25154         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25155         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25156         final NEDVelocity nedVelocity = new NEDVelocity();
25157         final ECEFPosition ecefPosition = new ECEFPosition();
25158         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25159         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25160                 ecefPosition, ecefVelocity);
25161         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25162                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25163         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25164 
25165         KnownGravityNormAccelerometerCalibrator calibrator =
25166                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
25167                         true, bx, by, bz, sx, sy, sz);
25168 
25169         // check default values
25170         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25171         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25172         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25173         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25174         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25175         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25176         final Acceleration bx2 = new Acceleration(0.0,
25177                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25178         calibrator.getInitialBiasXAsAcceleration(bx2);
25179         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25180         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25181         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25182         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25183         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25184         final Acceleration by2 = new Acceleration(0.0,
25185                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25186         calibrator.getInitialBiasYAsAcceleration(by2);
25187         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25188         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25189         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25190         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25191         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25192         final Acceleration bz2 = new Acceleration(0.0,
25193                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25194         calibrator.getInitialBiasZAsAcceleration(bz2);
25195         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25196         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25197         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25198         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25199         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25200         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25201         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25202         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25203         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25204         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25205         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25206         final double[] bias1 = calibrator.getInitialBias();
25207         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25208         final double[] bias2 = new double[3];
25209         calibrator.getInitialBias(bias2);
25210         assertArrayEquals(bias1, bias2, 0.0);
25211         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25212         assertEquals(b1, ba);
25213         final Matrix b2 = new Matrix(3, 1);
25214         calibrator.getInitialBiasAsMatrix(b2);
25215         assertEquals(b1, b2);
25216         final Matrix ma1 = calibrator.getInitialMa();
25217         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
25218         final Matrix ma2 = new Matrix(3, 3);
25219         calibrator.getInitialMa(ma2);
25220         assertEquals(ma1, ma2);
25221         assertNull(calibrator.getMeasurements());
25222         assertTrue(calibrator.isCommonAxisUsed());
25223         assertNull(calibrator.getListener());
25224         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25225         assertFalse(calibrator.isReady());
25226         assertFalse(calibrator.isRunning());
25227         assertNull(calibrator.getEstimatedBiases());
25228         assertFalse(calibrator.getEstimatedBiases(null));
25229         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25230         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25231         assertNull(calibrator.getEstimatedBiasFx());
25232         assertNull(calibrator.getEstimatedBiasFy());
25233         assertNull(calibrator.getEstimatedBiasFz());
25234         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25235         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25236         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25237         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25238         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25239         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25240         assertNull(calibrator.getEstimatedMa());
25241         assertNull(calibrator.getEstimatedSx());
25242         assertNull(calibrator.getEstimatedSy());
25243         assertNull(calibrator.getEstimatedSz());
25244         assertNull(calibrator.getEstimatedMxy());
25245         assertNull(calibrator.getEstimatedMxz());
25246         assertNull(calibrator.getEstimatedMyx());
25247         assertNull(calibrator.getEstimatedMyz());
25248         assertNull(calibrator.getEstimatedMzx());
25249         assertNull(calibrator.getEstimatedMzy());
25250         assertNull(calibrator.getEstimatedCovariance());
25251         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25252         assertNotNull(calibrator.getGroundTruthGravityNorm());
25253         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25254         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25255         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25256                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25257         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25258         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25259         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25260 
25261         // Force IllegalArgumentException
25262         final Acceleration invalidGravityNorm = new Acceleration(
25263                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25264 
25265         calibrator = null;
25266         try {
25267             calibrator = new KnownGravityNormAccelerometerCalibrator(
25268                     invalidGravityNorm, true,
25269                     bx, by, bz, sx, sy, sz);
25270             fail("IllegalArgumentException expected but not thrown");
25271         } catch (final IllegalArgumentException ignore) {
25272         }
25273         assertNull(calibrator);
25274     }
25275 
25276     @Test
25277     public void testConstructor193() throws WrongSizeException {
25278         final Matrix ba = generateBa();
25279         final double biasX = ba.getElementAtIndex(0);
25280         final double biasY = ba.getElementAtIndex(1);
25281         final double biasZ = ba.getElementAtIndex(2);
25282 
25283         final Acceleration bx = new Acceleration(biasX,
25284                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25285         final Acceleration by = new Acceleration(biasY,
25286                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25287         final Acceleration bz = new Acceleration(biasZ,
25288                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25289 
25290         final Matrix ma = generateMaCommonAxis();
25291         final double sx = ma.getElementAt(0, 0);
25292         final double sy = ma.getElementAt(1, 1);
25293         final double sz = ma.getElementAt(2, 2);
25294 
25295         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25296         final double latitude = Math.toRadians(
25297                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25298         final double longitude = Math.toRadians(
25299                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25300         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25301         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25302         final NEDVelocity nedVelocity = new NEDVelocity();
25303         final ECEFPosition ecefPosition = new ECEFPosition();
25304         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25305         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25306                 ecefPosition, ecefVelocity);
25307         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25308                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25309         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25310 
25311         KnownGravityNormAccelerometerCalibrator calibrator =
25312                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
25313                         true, bx, by, bz, sx, sy, sz, this);
25314 
25315         // check default values
25316         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25317         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25318         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25319         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25320         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25321         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25322         final Acceleration bx2 = new Acceleration(0.0,
25323                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25324         calibrator.getInitialBiasXAsAcceleration(bx2);
25325         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25326         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25327         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25328         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25329         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25330         final Acceleration by2 = new Acceleration(0.0,
25331                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25332         calibrator.getInitialBiasYAsAcceleration(by2);
25333         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25334         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25335         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25336         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25337         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25338         final Acceleration bz2 = new Acceleration(0.0,
25339                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25340         calibrator.getInitialBiasZAsAcceleration(bz2);
25341         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25342         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25343         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25344         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25345         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25346         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25347         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25348         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25349         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25350         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25351         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25352         final double[] bias1 = calibrator.getInitialBias();
25353         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25354         final double[] bias2 = new double[3];
25355         calibrator.getInitialBias(bias2);
25356         assertArrayEquals(bias1, bias2, 0.0);
25357         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25358         assertEquals(b1, ba);
25359         final Matrix b2 = new Matrix(3, 1);
25360         calibrator.getInitialBiasAsMatrix(b2);
25361         assertEquals(b1, b2);
25362         final Matrix ma1 = calibrator.getInitialMa();
25363         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
25364         final Matrix ma2 = new Matrix(3, 3);
25365         calibrator.getInitialMa(ma2);
25366         assertEquals(ma1, ma2);
25367         assertNull(calibrator.getMeasurements());
25368         assertTrue(calibrator.isCommonAxisUsed());
25369         assertSame(calibrator.getListener(), this);
25370         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25371         assertFalse(calibrator.isReady());
25372         assertFalse(calibrator.isRunning());
25373         assertNull(calibrator.getEstimatedBiases());
25374         assertFalse(calibrator.getEstimatedBiases(null));
25375         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25376         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25377         assertNull(calibrator.getEstimatedBiasFx());
25378         assertNull(calibrator.getEstimatedBiasFy());
25379         assertNull(calibrator.getEstimatedBiasFz());
25380         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25381         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25382         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25383         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25384         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25385         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25386         assertNull(calibrator.getEstimatedMa());
25387         assertNull(calibrator.getEstimatedSx());
25388         assertNull(calibrator.getEstimatedSy());
25389         assertNull(calibrator.getEstimatedSz());
25390         assertNull(calibrator.getEstimatedMxy());
25391         assertNull(calibrator.getEstimatedMxz());
25392         assertNull(calibrator.getEstimatedMyx());
25393         assertNull(calibrator.getEstimatedMyz());
25394         assertNull(calibrator.getEstimatedMzx());
25395         assertNull(calibrator.getEstimatedMzy());
25396         assertNull(calibrator.getEstimatedCovariance());
25397         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25398         assertNotNull(calibrator.getGroundTruthGravityNorm());
25399         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25400         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25401         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25402                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25403         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25404         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25405         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25406 
25407         // Force IllegalArgumentException
25408         final Acceleration invalidGravityNorm = new Acceleration(
25409                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25410 
25411         calibrator = null;
25412         try {
25413             calibrator = new KnownGravityNormAccelerometerCalibrator(
25414                     invalidGravityNorm, true,
25415                     bx, by, bz, sx, sy, sz, this);
25416             fail("IllegalArgumentException expected but not thrown");
25417         } catch (final IllegalArgumentException ignore) {
25418         }
25419         assertNull(calibrator);
25420     }
25421 
25422     @Test
25423     public void testConstructor194() throws WrongSizeException {
25424         final Collection<StandardDeviationBodyKinematics> measurements =
25425                 Collections.emptyList();
25426 
25427         final Matrix ba = generateBa();
25428         final double biasX = ba.getElementAtIndex(0);
25429         final double biasY = ba.getElementAtIndex(1);
25430         final double biasZ = ba.getElementAtIndex(2);
25431 
25432         final Acceleration bx = new Acceleration(biasX,
25433                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25434         final Acceleration by = new Acceleration(biasY,
25435                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25436         final Acceleration bz = new Acceleration(biasZ,
25437                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25438 
25439         final Matrix ma = generateMaCommonAxis();
25440         final double sx = ma.getElementAt(0, 0);
25441         final double sy = ma.getElementAt(1, 1);
25442         final double sz = ma.getElementAt(2, 2);
25443 
25444         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25445         final double latitude = Math.toRadians(
25446                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25447         final double longitude = Math.toRadians(
25448                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25449         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25450         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25451         final NEDVelocity nedVelocity = new NEDVelocity();
25452         final ECEFPosition ecefPosition = new ECEFPosition();
25453         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25454         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25455                 ecefPosition, ecefVelocity);
25456         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25457                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25458         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25459 
25460         KnownGravityNormAccelerometerCalibrator calibrator =
25461                 new KnownGravityNormAccelerometerCalibrator(
25462                         gravityNorm, measurements,
25463                         true, bx, by, bz, sx, sy, sz);
25464 
25465         // check default values
25466         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25467         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25468         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25469         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25470         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25471         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25472         final Acceleration bx2 = new Acceleration(0.0,
25473                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25474         calibrator.getInitialBiasXAsAcceleration(bx2);
25475         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25476         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25477         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25478         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25479         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25480         final Acceleration by2 = new Acceleration(0.0,
25481                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25482         calibrator.getInitialBiasYAsAcceleration(by2);
25483         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25484         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25485         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25486         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25487         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25488         final Acceleration bz2 = new Acceleration(0.0,
25489                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25490         calibrator.getInitialBiasZAsAcceleration(bz2);
25491         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25492         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25493         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25494         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25495         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25496         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25497         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25498         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25499         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25500         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25501         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25502         final double[] bias1 = calibrator.getInitialBias();
25503         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25504         final double[] bias2 = new double[3];
25505         calibrator.getInitialBias(bias2);
25506         assertArrayEquals(bias1, bias2, 0.0);
25507         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25508         assertEquals(b1, ba);
25509         final Matrix b2 = new Matrix(3, 1);
25510         calibrator.getInitialBiasAsMatrix(b2);
25511         assertEquals(b1, b2);
25512         final Matrix ma1 = calibrator.getInitialMa();
25513         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
25514         final Matrix ma2 = new Matrix(3, 3);
25515         calibrator.getInitialMa(ma2);
25516         assertEquals(ma1, ma2);
25517         assertSame(calibrator.getMeasurements(), measurements);
25518         assertTrue(calibrator.isCommonAxisUsed());
25519         assertNull(calibrator.getListener());
25520         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25521         assertFalse(calibrator.isReady());
25522         assertFalse(calibrator.isRunning());
25523         assertNull(calibrator.getEstimatedBiases());
25524         assertFalse(calibrator.getEstimatedBiases(null));
25525         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25526         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25527         assertNull(calibrator.getEstimatedBiasFx());
25528         assertNull(calibrator.getEstimatedBiasFy());
25529         assertNull(calibrator.getEstimatedBiasFz());
25530         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25531         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25532         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25533         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25534         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25535         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25536         assertNull(calibrator.getEstimatedMa());
25537         assertNull(calibrator.getEstimatedSx());
25538         assertNull(calibrator.getEstimatedSy());
25539         assertNull(calibrator.getEstimatedSz());
25540         assertNull(calibrator.getEstimatedMxy());
25541         assertNull(calibrator.getEstimatedMxz());
25542         assertNull(calibrator.getEstimatedMyx());
25543         assertNull(calibrator.getEstimatedMyz());
25544         assertNull(calibrator.getEstimatedMzx());
25545         assertNull(calibrator.getEstimatedMzy());
25546         assertNull(calibrator.getEstimatedCovariance());
25547         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25548         assertNotNull(calibrator.getGroundTruthGravityNorm());
25549         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25550         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25551         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25552                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25553         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25554         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25555         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25556 
25557         // Force IllegalArgumentException
25558         final Acceleration invalidGravityNorm = new Acceleration(
25559                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25560 
25561         calibrator = null;
25562         try {
25563             calibrator = new KnownGravityNormAccelerometerCalibrator(
25564                     invalidGravityNorm, measurements,
25565                     true, bx, by, bz, sx, sy, sz);
25566             fail("IllegalArgumentException expected but not thrown");
25567         } catch (final IllegalArgumentException ignore) {
25568         }
25569         assertNull(calibrator);
25570     }
25571 
25572     @Test
25573     public void testConstructor195() throws WrongSizeException {
25574         final Collection<StandardDeviationBodyKinematics> measurements =
25575                 Collections.emptyList();
25576 
25577         final Matrix ba = generateBa();
25578         final double biasX = ba.getElementAtIndex(0);
25579         final double biasY = ba.getElementAtIndex(1);
25580         final double biasZ = ba.getElementAtIndex(2);
25581 
25582         final Acceleration bx = new Acceleration(biasX,
25583                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25584         final Acceleration by = new Acceleration(biasY,
25585                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25586         final Acceleration bz = new Acceleration(biasZ,
25587                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
25588 
25589         final Matrix ma = generateMaCommonAxis();
25590         final double sx = ma.getElementAt(0, 0);
25591         final double sy = ma.getElementAt(1, 1);
25592         final double sz = ma.getElementAt(2, 2);
25593 
25594         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25595         final double latitude = Math.toRadians(
25596                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25597         final double longitude = Math.toRadians(
25598                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25599         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25600         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25601         final NEDVelocity nedVelocity = new NEDVelocity();
25602         final ECEFPosition ecefPosition = new ECEFPosition();
25603         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25604         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25605                 ecefPosition, ecefVelocity);
25606         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25607                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25608         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25609 
25610         KnownGravityNormAccelerometerCalibrator calibrator =
25611                 new KnownGravityNormAccelerometerCalibrator(
25612                         gravityNorm, measurements,
25613                         true, bx, by, bz, sx, sy, sz, this);
25614 
25615         // check default values
25616         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25617         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25618         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25619         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25620         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25621         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25622         final Acceleration bx2 = new Acceleration(0.0,
25623                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25624         calibrator.getInitialBiasXAsAcceleration(bx2);
25625         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25626         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25627         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25628         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25629         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25630         final Acceleration by2 = new Acceleration(0.0,
25631                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25632         calibrator.getInitialBiasYAsAcceleration(by2);
25633         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25634         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25635         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25636         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25637         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25638         final Acceleration bz2 = new Acceleration(0.0,
25639                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25640         calibrator.getInitialBiasZAsAcceleration(bz2);
25641         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25642         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25643         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25644         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25645         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25646         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
25647         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
25648         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
25649         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
25650         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
25651         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
25652         final double[] bias1 = calibrator.getInitialBias();
25653         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25654         final double[] bias2 = new double[3];
25655         calibrator.getInitialBias(bias2);
25656         assertArrayEquals(bias1, bias2, 0.0);
25657         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25658         assertEquals(b1, ba);
25659         final Matrix b2 = new Matrix(3, 1);
25660         calibrator.getInitialBiasAsMatrix(b2);
25661         assertEquals(b1, b2);
25662         final Matrix ma1 = calibrator.getInitialMa();
25663         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
25664         final Matrix ma2 = new Matrix(3, 3);
25665         calibrator.getInitialMa(ma2);
25666         assertEquals(ma1, ma2);
25667         assertSame(calibrator.getMeasurements(), measurements);
25668         assertTrue(calibrator.isCommonAxisUsed());
25669         assertSame(calibrator.getListener(), this);
25670         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25671         assertFalse(calibrator.isReady());
25672         assertFalse(calibrator.isRunning());
25673         assertNull(calibrator.getEstimatedBiases());
25674         assertFalse(calibrator.getEstimatedBiases(null));
25675         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25676         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25677         assertNull(calibrator.getEstimatedBiasFx());
25678         assertNull(calibrator.getEstimatedBiasFy());
25679         assertNull(calibrator.getEstimatedBiasFz());
25680         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25681         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25682         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25683         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25684         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25685         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25686         assertNull(calibrator.getEstimatedMa());
25687         assertNull(calibrator.getEstimatedSx());
25688         assertNull(calibrator.getEstimatedSy());
25689         assertNull(calibrator.getEstimatedSz());
25690         assertNull(calibrator.getEstimatedMxy());
25691         assertNull(calibrator.getEstimatedMxz());
25692         assertNull(calibrator.getEstimatedMyx());
25693         assertNull(calibrator.getEstimatedMyz());
25694         assertNull(calibrator.getEstimatedMzx());
25695         assertNull(calibrator.getEstimatedMzy());
25696         assertNull(calibrator.getEstimatedCovariance());
25697         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25698         assertNotNull(calibrator.getGroundTruthGravityNorm());
25699         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25700         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25701         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25702                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25703         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25704         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25705         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25706 
25707         // Force IllegalArgumentException
25708         final Acceleration invalidGravityNorm = new Acceleration(
25709                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25710 
25711         calibrator = null;
25712         try {
25713             calibrator = new KnownGravityNormAccelerometerCalibrator(
25714                     invalidGravityNorm, measurements,
25715                     true, bx, by, bz, sx, sy, sz, this);
25716             fail("IllegalArgumentException expected but not thrown");
25717         } catch (final IllegalArgumentException ignore) {
25718         }
25719         assertNull(calibrator);
25720     }
25721 
25722     @Test
25723     public void testConstructor196() throws WrongSizeException {
25724         final Matrix ba = generateBa();
25725         final double biasX = ba.getElementAtIndex(0);
25726         final double biasY = ba.getElementAtIndex(1);
25727         final double biasZ = ba.getElementAtIndex(2);
25728 
25729         final Matrix ma = generateMaCommonAxis();
25730         final double sx = ma.getElementAt(0, 0);
25731         final double sy = ma.getElementAt(1, 1);
25732         final double sz = ma.getElementAt(2, 2);
25733         final double mxy = ma.getElementAt(0, 1);
25734         final double mxz = ma.getElementAt(0, 2);
25735         final double myx = ma.getElementAt(1, 0);
25736         final double myz = ma.getElementAt(1, 2);
25737         final double mzx = ma.getElementAt(2, 0);
25738         final double mzy = ma.getElementAt(2, 1);
25739 
25740         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25741         final double latitude = Math.toRadians(
25742                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25743         final double longitude = Math.toRadians(
25744                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25745         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25746         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25747         final NEDVelocity nedVelocity = new NEDVelocity();
25748         final ECEFPosition ecefPosition = new ECEFPosition();
25749         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25750         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25751                 ecefPosition, ecefVelocity);
25752         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25753                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25754         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25755 
25756         KnownGravityNormAccelerometerCalibrator calibrator =
25757                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
25758                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25759                         myx, myz, mzx, mzy);
25760 
25761         // check default values
25762         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25763         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25764         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25765         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25766         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25767         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25768         final Acceleration bx2 = new Acceleration(0.0,
25769                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25770         calibrator.getInitialBiasXAsAcceleration(bx2);
25771         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25772         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25773         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25774         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25775         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25776         final Acceleration by2 = new Acceleration(0.0,
25777                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25778         calibrator.getInitialBiasYAsAcceleration(by2);
25779         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25780         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25781         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25782         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25783         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25784         final Acceleration bz2 = new Acceleration(0.0,
25785                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25786         calibrator.getInitialBiasZAsAcceleration(bz2);
25787         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25788         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25789         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25790         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25791         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25792         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25793         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25794         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25795         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25796         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25797         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25798         final double[] bias1 = calibrator.getInitialBias();
25799         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25800         final double[] bias2 = new double[3];
25801         calibrator.getInitialBias(bias2);
25802         assertArrayEquals(bias1, bias2, 0.0);
25803         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25804         assertEquals(b1, ba);
25805         final Matrix b2 = new Matrix(3, 1);
25806         calibrator.getInitialBiasAsMatrix(b2);
25807         assertEquals(b1, b2);
25808         final Matrix ma1 = new Matrix(3, 3);
25809         ma1.setSubmatrix(0, 0,
25810                 2, 2,
25811                 new double[]{sx, myx, mzx,
25812                         mxy, sy, mzy,
25813                         mxz, myz, sz});
25814         assertEquals(calibrator.getInitialMa(), ma1);
25815         final Matrix ma2 = new Matrix(3, 3);
25816         calibrator.getInitialMa(ma2);
25817         assertEquals(ma1, ma2);
25818         assertNull(calibrator.getMeasurements());
25819         assertFalse(calibrator.isCommonAxisUsed());
25820         assertNull(calibrator.getListener());
25821         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25822         assertFalse(calibrator.isReady());
25823         assertFalse(calibrator.isRunning());
25824         assertNull(calibrator.getEstimatedBiases());
25825         assertFalse(calibrator.getEstimatedBiases(null));
25826         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25827         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25828         assertNull(calibrator.getEstimatedBiasFx());
25829         assertNull(calibrator.getEstimatedBiasFy());
25830         assertNull(calibrator.getEstimatedBiasFz());
25831         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25832         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25833         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25834         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25835         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25836         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25837         assertNull(calibrator.getEstimatedMa());
25838         assertNull(calibrator.getEstimatedSx());
25839         assertNull(calibrator.getEstimatedSy());
25840         assertNull(calibrator.getEstimatedSz());
25841         assertNull(calibrator.getEstimatedMxy());
25842         assertNull(calibrator.getEstimatedMxz());
25843         assertNull(calibrator.getEstimatedMyx());
25844         assertNull(calibrator.getEstimatedMyz());
25845         assertNull(calibrator.getEstimatedMzx());
25846         assertNull(calibrator.getEstimatedMzy());
25847         assertNull(calibrator.getEstimatedCovariance());
25848         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25849         assertNotNull(calibrator.getGroundTruthGravityNorm());
25850         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25851         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25852         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25853                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25854         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25855         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25856         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25857 
25858         // Force IllegalArgumentException
25859         final Acceleration invalidGravityNorm = new Acceleration(
25860                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25861 
25862         calibrator = null;
25863         try {
25864             calibrator = new KnownGravityNormAccelerometerCalibrator(
25865                     invalidGravityNorm, biasX, biasY, biasZ, sx, sy, sz,
25866                     mxy, mxz, myx, myz, mzx, mzy);
25867             fail("IllegalArgumentException expected but not thrown");
25868         } catch (final IllegalArgumentException ignore) {
25869         }
25870         assertNull(calibrator);
25871     }
25872 
25873     @Test
25874     public void testConstructor197() throws WrongSizeException {
25875         final Collection<StandardDeviationBodyKinematics> measurements =
25876                 Collections.emptyList();
25877 
25878         final Matrix ba = generateBa();
25879         final double biasX = ba.getElementAtIndex(0);
25880         final double biasY = ba.getElementAtIndex(1);
25881         final double biasZ = ba.getElementAtIndex(2);
25882 
25883         final Matrix ma = generateMaCommonAxis();
25884         final double sx = ma.getElementAt(0, 0);
25885         final double sy = ma.getElementAt(1, 1);
25886         final double sz = ma.getElementAt(2, 2);
25887         final double mxy = ma.getElementAt(0, 1);
25888         final double mxz = ma.getElementAt(0, 2);
25889         final double myx = ma.getElementAt(1, 0);
25890         final double myz = ma.getElementAt(1, 2);
25891         final double mzx = ma.getElementAt(2, 0);
25892         final double mzy = ma.getElementAt(2, 1);
25893 
25894         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25895         final double latitude = Math.toRadians(
25896                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25897         final double longitude = Math.toRadians(
25898                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25899         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25900         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25901         final NEDVelocity nedVelocity = new NEDVelocity();
25902         final ECEFPosition ecefPosition = new ECEFPosition();
25903         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25904         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25905                 ecefPosition, ecefVelocity);
25906         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25907                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25908         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
25909 
25910         KnownGravityNormAccelerometerCalibrator calibrator =
25911                 new KnownGravityNormAccelerometerCalibrator(
25912                         gravityNorm, measurements,
25913                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25914                         myx, myz, mzx, mzy);
25915 
25916         // check default values
25917         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25918         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25919         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25920         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25921         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25922         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25923         final Acceleration bx2 = new Acceleration(0.0,
25924                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25925         calibrator.getInitialBiasXAsAcceleration(bx2);
25926         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25927         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25928         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25929         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25930         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25931         final Acceleration by2 = new Acceleration(0.0,
25932                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25933         calibrator.getInitialBiasYAsAcceleration(by2);
25934         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25935         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25936         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25937         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25938         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25939         final Acceleration bz2 = new Acceleration(0.0,
25940                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25941         calibrator.getInitialBiasZAsAcceleration(bz2);
25942         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25943         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25944         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25945         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25946         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25947         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25948         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25949         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25950         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25951         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25952         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25953         final double[] bias1 = calibrator.getInitialBias();
25954         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25955         final double[] bias2 = new double[3];
25956         calibrator.getInitialBias(bias2);
25957         assertArrayEquals(bias1, bias2, 0.0);
25958         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25959         assertEquals(b1, ba);
25960         final Matrix b2 = new Matrix(3, 1);
25961         calibrator.getInitialBiasAsMatrix(b2);
25962         assertEquals(b1, b2);
25963         final Matrix ma1 = new Matrix(3, 3);
25964         ma1.setSubmatrix(0, 0,
25965                 2, 2,
25966                 new double[]{sx, myx, mzx,
25967                         mxy, sy, mzy,
25968                         mxz, myz, sz});
25969         assertEquals(calibrator.getInitialMa(), ma1);
25970         final Matrix ma2 = new Matrix(3, 3);
25971         calibrator.getInitialMa(ma2);
25972         assertEquals(ma1, ma2);
25973         assertSame(calibrator.getMeasurements(), measurements);
25974         assertFalse(calibrator.isCommonAxisUsed());
25975         assertNull(calibrator.getListener());
25976         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25977         assertFalse(calibrator.isReady());
25978         assertFalse(calibrator.isRunning());
25979         assertNull(calibrator.getEstimatedBiases());
25980         assertFalse(calibrator.getEstimatedBiases(null));
25981         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25982         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25983         assertNull(calibrator.getEstimatedBiasFx());
25984         assertNull(calibrator.getEstimatedBiasFy());
25985         assertNull(calibrator.getEstimatedBiasFz());
25986         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25987         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25988         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25989         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25990         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25991         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25992         assertNull(calibrator.getEstimatedMa());
25993         assertNull(calibrator.getEstimatedSx());
25994         assertNull(calibrator.getEstimatedSy());
25995         assertNull(calibrator.getEstimatedSz());
25996         assertNull(calibrator.getEstimatedMxy());
25997         assertNull(calibrator.getEstimatedMxz());
25998         assertNull(calibrator.getEstimatedMyx());
25999         assertNull(calibrator.getEstimatedMyz());
26000         assertNull(calibrator.getEstimatedMzx());
26001         assertNull(calibrator.getEstimatedMzy());
26002         assertNull(calibrator.getEstimatedCovariance());
26003         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26004         assertNotNull(calibrator.getGroundTruthGravityNorm());
26005         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26006         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26007         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26008                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26009         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26010         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26011         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26012 
26013         // Force IllegalArgumentException
26014         final Acceleration invalidGravityNorm = new Acceleration(
26015                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26016 
26017         calibrator = null;
26018         try {
26019             calibrator = new KnownGravityNormAccelerometerCalibrator(
26020                     invalidGravityNorm, measurements,
26021                     biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
26022                     myx, myz, mzx, mzy);
26023             fail("IllegalArgumentException expected but not thrown");
26024         } catch (final IllegalArgumentException ignore) {
26025         }
26026         assertNull(calibrator);
26027     }
26028 
26029     @Test
26030     public void testConstructor198() throws WrongSizeException {
26031         final Collection<StandardDeviationBodyKinematics> measurements =
26032                 Collections.emptyList();
26033 
26034         final Matrix ba = generateBa();
26035         final double biasX = ba.getElementAtIndex(0);
26036         final double biasY = ba.getElementAtIndex(1);
26037         final double biasZ = ba.getElementAtIndex(2);
26038 
26039         final Matrix ma = generateMaCommonAxis();
26040         final double sx = ma.getElementAt(0, 0);
26041         final double sy = ma.getElementAt(1, 1);
26042         final double sz = ma.getElementAt(2, 2);
26043         final double mxy = ma.getElementAt(0, 1);
26044         final double mxz = ma.getElementAt(0, 2);
26045         final double myx = ma.getElementAt(1, 0);
26046         final double myz = ma.getElementAt(1, 2);
26047         final double mzx = ma.getElementAt(2, 0);
26048         final double mzy = ma.getElementAt(2, 1);
26049 
26050         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26051         final double latitude = Math.toRadians(
26052                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26053         final double longitude = Math.toRadians(
26054                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26055         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26056         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26057         final NEDVelocity nedVelocity = new NEDVelocity();
26058         final ECEFPosition ecefPosition = new ECEFPosition();
26059         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26060         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26061                 ecefPosition, ecefVelocity);
26062         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26063                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26064         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26065 
26066         KnownGravityNormAccelerometerCalibrator calibrator =
26067                 new KnownGravityNormAccelerometerCalibrator(
26068                         gravityNorm, measurements,
26069                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
26070                         myx, myz, mzx, mzy, this);
26071 
26072         // check default values
26073         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26074         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26075         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26076         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26077         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26078         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26079         final Acceleration bx2 = new Acceleration(0.0,
26080                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26081         calibrator.getInitialBiasXAsAcceleration(bx2);
26082         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26083         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26084         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26085         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26086         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26087         final Acceleration by2 = new Acceleration(0.0,
26088                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26089         calibrator.getInitialBiasYAsAcceleration(by2);
26090         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26091         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26092         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26093         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26094         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26095         final Acceleration bz2 = new Acceleration(0.0,
26096                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26097         calibrator.getInitialBiasZAsAcceleration(bz2);
26098         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26099         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26100         assertEquals(calibrator.getInitialSx(), sx, 0.0);
26101         assertEquals(calibrator.getInitialSy(), sy, 0.0);
26102         assertEquals(calibrator.getInitialSz(), sz, 0.0);
26103         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26104         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26105         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26106         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26107         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26108         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26109         final double[] bias1 = calibrator.getInitialBias();
26110         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26111         final double[] bias2 = new double[3];
26112         calibrator.getInitialBias(bias2);
26113         assertArrayEquals(bias1, bias2, 0.0);
26114         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26115         assertEquals(b1, ba);
26116         final Matrix b2 = new Matrix(3, 1);
26117         calibrator.getInitialBiasAsMatrix(b2);
26118         assertEquals(b1, b2);
26119         final Matrix ma1 = new Matrix(3, 3);
26120         ma1.setSubmatrix(0, 0,
26121                 2, 2,
26122                 new double[]{sx, myx, mzx,
26123                         mxy, sy, mzy,
26124                         mxz, myz, sz});
26125         assertEquals(calibrator.getInitialMa(), ma1);
26126         final Matrix ma2 = new Matrix(3, 3);
26127         calibrator.getInitialMa(ma2);
26128         assertEquals(ma1, ma2);
26129         assertSame(calibrator.getMeasurements(), measurements);
26130         assertFalse(calibrator.isCommonAxisUsed());
26131         assertSame(calibrator.getListener(), this);
26132         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26133         assertFalse(calibrator.isReady());
26134         assertFalse(calibrator.isRunning());
26135         assertNull(calibrator.getEstimatedBiases());
26136         assertFalse(calibrator.getEstimatedBiases(null));
26137         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26138         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26139         assertNull(calibrator.getEstimatedBiasFx());
26140         assertNull(calibrator.getEstimatedBiasFy());
26141         assertNull(calibrator.getEstimatedBiasFz());
26142         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26143         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26144         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26145         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26146         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26147         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26148         assertNull(calibrator.getEstimatedMa());
26149         assertNull(calibrator.getEstimatedSx());
26150         assertNull(calibrator.getEstimatedSy());
26151         assertNull(calibrator.getEstimatedSz());
26152         assertNull(calibrator.getEstimatedMxy());
26153         assertNull(calibrator.getEstimatedMxz());
26154         assertNull(calibrator.getEstimatedMyx());
26155         assertNull(calibrator.getEstimatedMyz());
26156         assertNull(calibrator.getEstimatedMzx());
26157         assertNull(calibrator.getEstimatedMzy());
26158         assertNull(calibrator.getEstimatedCovariance());
26159         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26160         assertNotNull(calibrator.getGroundTruthGravityNorm());
26161         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26162         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26163         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26164                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26165         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26166         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26167         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26168 
26169         // Force IllegalArgumentException
26170         final Acceleration invalidGravityNorm = new Acceleration(
26171                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26172 
26173         calibrator = null;
26174         try {
26175             calibrator = new KnownGravityNormAccelerometerCalibrator(
26176                     invalidGravityNorm, measurements,
26177                     biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
26178                     myx, myz, mzx, mzy, this);
26179             fail("IllegalArgumentException expected but not thrown");
26180         } catch (final IllegalArgumentException ignore) {
26181         }
26182         assertNull(calibrator);
26183     }
26184 
26185     @Test
26186     public void testConstructor199() throws WrongSizeException {
26187         final Matrix ba = generateBa();
26188         final double biasX = ba.getElementAtIndex(0);
26189         final double biasY = ba.getElementAtIndex(1);
26190         final double biasZ = ba.getElementAtIndex(2);
26191 
26192         final Matrix ma = generateMaCommonAxis();
26193         final double sx = ma.getElementAt(0, 0);
26194         final double sy = ma.getElementAt(1, 1);
26195         final double sz = ma.getElementAt(2, 2);
26196         final double mxy = ma.getElementAt(0, 1);
26197         final double mxz = ma.getElementAt(0, 2);
26198         final double myx = ma.getElementAt(1, 0);
26199         final double myz = ma.getElementAt(1, 2);
26200         final double mzx = ma.getElementAt(2, 0);
26201         final double mzy = ma.getElementAt(2, 1);
26202 
26203         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26204         final double latitude = Math.toRadians(
26205                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26206         final double longitude = Math.toRadians(
26207                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26208         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26209         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26210         final NEDVelocity nedVelocity = new NEDVelocity();
26211         final ECEFPosition ecefPosition = new ECEFPosition();
26212         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26213         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26214                 ecefPosition, ecefVelocity);
26215         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26216                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26217         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26218 
26219         KnownGravityNormAccelerometerCalibrator calibrator =
26220                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
26221                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
26222                         myx, myz, mzx, mzy);
26223 
26224         // check default values
26225         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26226         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26227         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26228         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26229         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26230         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26231         final Acceleration bx2 = new Acceleration(0.0,
26232                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26233         calibrator.getInitialBiasXAsAcceleration(bx2);
26234         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26235         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26236         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26237         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26238         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26239         final Acceleration by2 = new Acceleration(0.0,
26240                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26241         calibrator.getInitialBiasYAsAcceleration(by2);
26242         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26243         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26244         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26245         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26246         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26247         final Acceleration bz2 = new Acceleration(0.0,
26248                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26249         calibrator.getInitialBiasZAsAcceleration(bz2);
26250         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26251         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26252         assertEquals(calibrator.getInitialSx(), sx, 0.0);
26253         assertEquals(calibrator.getInitialSy(), sy, 0.0);
26254         assertEquals(calibrator.getInitialSz(), sz, 0.0);
26255         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26256         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26257         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26258         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26259         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26260         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26261         final double[] bias1 = calibrator.getInitialBias();
26262         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26263         final double[] bias2 = new double[3];
26264         calibrator.getInitialBias(bias2);
26265         assertArrayEquals(bias1, bias2, 0.0);
26266         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26267         assertEquals(b1, ba);
26268         final Matrix b2 = new Matrix(3, 1);
26269         calibrator.getInitialBiasAsMatrix(b2);
26270         assertEquals(b1, b2);
26271         final Matrix ma1 = new Matrix(3, 3);
26272         ma1.setSubmatrix(0, 0,
26273                 2, 2,
26274                 new double[]{sx, myx, mzx,
26275                         mxy, sy, mzy,
26276                         mxz, myz, sz});
26277         assertEquals(calibrator.getInitialMa(), ma1);
26278         final Matrix ma2 = new Matrix(3, 3);
26279         calibrator.getInitialMa(ma2);
26280         assertEquals(ma1, ma2);
26281         assertNull(calibrator.getMeasurements());
26282         assertTrue(calibrator.isCommonAxisUsed());
26283         assertNull(calibrator.getListener());
26284         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26285         assertFalse(calibrator.isReady());
26286         assertFalse(calibrator.isRunning());
26287         assertNull(calibrator.getEstimatedBiases());
26288         assertFalse(calibrator.getEstimatedBiases(null));
26289         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26290         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26291         assertNull(calibrator.getEstimatedBiasFx());
26292         assertNull(calibrator.getEstimatedBiasFy());
26293         assertNull(calibrator.getEstimatedBiasFz());
26294         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26295         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26296         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26297         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26298         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26299         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26300         assertNull(calibrator.getEstimatedMa());
26301         assertNull(calibrator.getEstimatedSx());
26302         assertNull(calibrator.getEstimatedSy());
26303         assertNull(calibrator.getEstimatedSz());
26304         assertNull(calibrator.getEstimatedMxy());
26305         assertNull(calibrator.getEstimatedMxz());
26306         assertNull(calibrator.getEstimatedMyx());
26307         assertNull(calibrator.getEstimatedMyz());
26308         assertNull(calibrator.getEstimatedMzx());
26309         assertNull(calibrator.getEstimatedMzy());
26310         assertNull(calibrator.getEstimatedCovariance());
26311         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26312         assertNotNull(calibrator.getGroundTruthGravityNorm());
26313         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26314         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26315         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26316                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26317         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26318         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26319         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26320 
26321         // Force IllegalArgumentException
26322         final Acceleration invalidGravityNorm = new Acceleration(
26323                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26324 
26325         calibrator = null;
26326         try {
26327             calibrator = new KnownGravityNormAccelerometerCalibrator(
26328                     invalidGravityNorm, true, biasX, biasY, biasZ,
26329                     sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
26330             fail("IllegalArgumentException expected but not thrown");
26331         } catch (final IllegalArgumentException ignore) {
26332         }
26333         assertNull(calibrator);
26334     }
26335 
26336     @Test
26337     public void testConstructor200() throws WrongSizeException {
26338         final Matrix ba = generateBa();
26339         final double biasX = ba.getElementAtIndex(0);
26340         final double biasY = ba.getElementAtIndex(1);
26341         final double biasZ = ba.getElementAtIndex(2);
26342 
26343         final Matrix ma = generateMaCommonAxis();
26344         final double sx = ma.getElementAt(0, 0);
26345         final double sy = ma.getElementAt(1, 1);
26346         final double sz = ma.getElementAt(2, 2);
26347         final double mxy = ma.getElementAt(0, 1);
26348         final double mxz = ma.getElementAt(0, 2);
26349         final double myx = ma.getElementAt(1, 0);
26350         final double myz = ma.getElementAt(1, 2);
26351         final double mzx = ma.getElementAt(2, 0);
26352         final double mzy = ma.getElementAt(2, 1);
26353 
26354         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26355         final double latitude = Math.toRadians(
26356                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26357         final double longitude = Math.toRadians(
26358                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26359         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26360         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26361         final NEDVelocity nedVelocity = new NEDVelocity();
26362         final ECEFPosition ecefPosition = new ECEFPosition();
26363         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26364         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26365                 ecefPosition, ecefVelocity);
26366         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26367                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26368         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26369 
26370         KnownGravityNormAccelerometerCalibrator calibrator =
26371                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
26372                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
26373                         myx, myz, mzx, mzy, this);
26374 
26375         // check default values
26376         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26377         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26378         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26379         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26380         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26381         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26382         final Acceleration bx2 = new Acceleration(0.0,
26383                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26384         calibrator.getInitialBiasXAsAcceleration(bx2);
26385         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26386         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26387         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26388         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26389         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26390         final Acceleration by2 = new Acceleration(0.0,
26391                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26392         calibrator.getInitialBiasYAsAcceleration(by2);
26393         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26394         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26395         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26396         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26397         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26398         final Acceleration bz2 = new Acceleration(0.0,
26399                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26400         calibrator.getInitialBiasZAsAcceleration(bz2);
26401         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26402         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26403         assertEquals(calibrator.getInitialSx(), sx, 0.0);
26404         assertEquals(calibrator.getInitialSy(), sy, 0.0);
26405         assertEquals(calibrator.getInitialSz(), sz, 0.0);
26406         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26407         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26408         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26409         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26410         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26411         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26412         final double[] bias1 = calibrator.getInitialBias();
26413         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26414         final double[] bias2 = new double[3];
26415         calibrator.getInitialBias(bias2);
26416         assertArrayEquals(bias1, bias2, 0.0);
26417         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26418         assertEquals(b1, ba);
26419         final Matrix b2 = new Matrix(3, 1);
26420         calibrator.getInitialBiasAsMatrix(b2);
26421         assertEquals(b1, b2);
26422         final Matrix ma1 = new Matrix(3, 3);
26423         ma1.setSubmatrix(0, 0,
26424                 2, 2,
26425                 new double[]{sx, myx, mzx,
26426                         mxy, sy, mzy,
26427                         mxz, myz, sz});
26428         assertEquals(calibrator.getInitialMa(), ma1);
26429         final Matrix ma2 = new Matrix(3, 3);
26430         calibrator.getInitialMa(ma2);
26431         assertEquals(ma1, ma2);
26432         assertNull(calibrator.getMeasurements());
26433         assertTrue(calibrator.isCommonAxisUsed());
26434         assertSame(calibrator.getListener(), this);
26435         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26436         assertFalse(calibrator.isReady());
26437         assertFalse(calibrator.isRunning());
26438         assertNull(calibrator.getEstimatedBiases());
26439         assertFalse(calibrator.getEstimatedBiases(null));
26440         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26441         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26442         assertNull(calibrator.getEstimatedBiasFx());
26443         assertNull(calibrator.getEstimatedBiasFy());
26444         assertNull(calibrator.getEstimatedBiasFz());
26445         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26446         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26447         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26448         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26449         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26450         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26451         assertNull(calibrator.getEstimatedMa());
26452         assertNull(calibrator.getEstimatedSx());
26453         assertNull(calibrator.getEstimatedSy());
26454         assertNull(calibrator.getEstimatedSz());
26455         assertNull(calibrator.getEstimatedMxy());
26456         assertNull(calibrator.getEstimatedMxz());
26457         assertNull(calibrator.getEstimatedMyx());
26458         assertNull(calibrator.getEstimatedMyz());
26459         assertNull(calibrator.getEstimatedMzx());
26460         assertNull(calibrator.getEstimatedMzy());
26461         assertNull(calibrator.getEstimatedCovariance());
26462         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26463         assertNotNull(calibrator.getGroundTruthGravityNorm());
26464         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26465         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26466         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26467                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26468         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26469         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26470         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26471 
26472         // Force IllegalArgumentException
26473         final Acceleration invalidGravityNorm = new Acceleration(
26474                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26475 
26476         calibrator = null;
26477         try {
26478             calibrator = new KnownGravityNormAccelerometerCalibrator(
26479                     invalidGravityNorm, true, biasX, biasY, biasZ,
26480                     sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
26481                     this);
26482             fail("IllegalArgumentException expected but not thrown");
26483         } catch (final IllegalArgumentException ignore) {
26484         }
26485         assertNull(calibrator);
26486     }
26487 
26488     @Test
26489     public void testConstructor201() throws WrongSizeException {
26490         final Collection<StandardDeviationBodyKinematics> measurements =
26491                 Collections.emptyList();
26492 
26493         final Matrix ba = generateBa();
26494         final double biasX = ba.getElementAtIndex(0);
26495         final double biasY = ba.getElementAtIndex(1);
26496         final double biasZ = ba.getElementAtIndex(2);
26497 
26498         final Matrix ma = generateMaCommonAxis();
26499         final double sx = ma.getElementAt(0, 0);
26500         final double sy = ma.getElementAt(1, 1);
26501         final double sz = ma.getElementAt(2, 2);
26502         final double mxy = ma.getElementAt(0, 1);
26503         final double mxz = ma.getElementAt(0, 2);
26504         final double myx = ma.getElementAt(1, 0);
26505         final double myz = ma.getElementAt(1, 2);
26506         final double mzx = ma.getElementAt(2, 0);
26507         final double mzy = ma.getElementAt(2, 1);
26508 
26509         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26510         final double latitude = Math.toRadians(
26511                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26512         final double longitude = Math.toRadians(
26513                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26514         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26515         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26516         final NEDVelocity nedVelocity = new NEDVelocity();
26517         final ECEFPosition ecefPosition = new ECEFPosition();
26518         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26519         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26520                 ecefPosition, ecefVelocity);
26521         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26522                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26523         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26524 
26525         KnownGravityNormAccelerometerCalibrator calibrator =
26526                 new KnownGravityNormAccelerometerCalibrator(
26527                         gravityNorm, measurements,
26528                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
26529                         myx, myz, mzx, mzy);
26530 
26531         // check default values
26532         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26533         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26534         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26535         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26536         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26537         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26538         final Acceleration bx2 = new Acceleration(0.0,
26539                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26540         calibrator.getInitialBiasXAsAcceleration(bx2);
26541         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26542         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26543         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26544         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26545         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26546         final Acceleration by2 = new Acceleration(0.0,
26547                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26548         calibrator.getInitialBiasYAsAcceleration(by2);
26549         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26550         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26551         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26552         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26553         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26554         final Acceleration bz2 = new Acceleration(0.0,
26555                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26556         calibrator.getInitialBiasZAsAcceleration(bz2);
26557         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26558         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26559         assertEquals(calibrator.getInitialSx(), sx, 0.0);
26560         assertEquals(calibrator.getInitialSy(), sy, 0.0);
26561         assertEquals(calibrator.getInitialSz(), sz, 0.0);
26562         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26563         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26564         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26565         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26566         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26567         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26568         final double[] bias1 = calibrator.getInitialBias();
26569         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26570         final double[] bias2 = new double[3];
26571         calibrator.getInitialBias(bias2);
26572         assertArrayEquals(bias1, bias2, 0.0);
26573         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26574         assertEquals(b1, ba);
26575         final Matrix b2 = new Matrix(3, 1);
26576         calibrator.getInitialBiasAsMatrix(b2);
26577         assertEquals(b1, b2);
26578         final Matrix ma1 = new Matrix(3, 3);
26579         ma1.setSubmatrix(0, 0,
26580                 2, 2,
26581                 new double[]{sx, myx, mzx,
26582                         mxy, sy, mzy,
26583                         mxz, myz, sz});
26584         assertEquals(calibrator.getInitialMa(), ma1);
26585         final Matrix ma2 = new Matrix(3, 3);
26586         calibrator.getInitialMa(ma2);
26587         assertEquals(ma1, ma2);
26588         assertSame(calibrator.getMeasurements(), measurements);
26589         assertTrue(calibrator.isCommonAxisUsed());
26590         assertNull(calibrator.getListener());
26591         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26592         assertFalse(calibrator.isReady());
26593         assertFalse(calibrator.isRunning());
26594         assertNull(calibrator.getEstimatedBiases());
26595         assertFalse(calibrator.getEstimatedBiases(null));
26596         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26597         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26598         assertNull(calibrator.getEstimatedBiasFx());
26599         assertNull(calibrator.getEstimatedBiasFy());
26600         assertNull(calibrator.getEstimatedBiasFz());
26601         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26602         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26603         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26604         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26605         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26606         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26607         assertNull(calibrator.getEstimatedMa());
26608         assertNull(calibrator.getEstimatedSx());
26609         assertNull(calibrator.getEstimatedSy());
26610         assertNull(calibrator.getEstimatedSz());
26611         assertNull(calibrator.getEstimatedMxy());
26612         assertNull(calibrator.getEstimatedMxz());
26613         assertNull(calibrator.getEstimatedMyx());
26614         assertNull(calibrator.getEstimatedMyz());
26615         assertNull(calibrator.getEstimatedMzx());
26616         assertNull(calibrator.getEstimatedMzy());
26617         assertNull(calibrator.getEstimatedCovariance());
26618         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26619         assertNotNull(calibrator.getGroundTruthGravityNorm());
26620         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26621         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26622         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26623                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26624         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26625         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26626         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26627 
26628         // Force IllegalArgumentException
26629         final Acceleration invalidGravityNorm = new Acceleration(
26630                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26631 
26632         calibrator = null;
26633         try {
26634             calibrator = new KnownGravityNormAccelerometerCalibrator(
26635                     invalidGravityNorm, measurements,
26636                     true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
26637                     myx, myz, mzx, mzy);
26638             fail("IllegalArgumentException expected but not thrown");
26639         } catch (final IllegalArgumentException ignore) {
26640         }
26641         assertNull(calibrator);
26642     }
26643 
26644     @Test
26645     public void testConstructor202() throws WrongSizeException {
26646         final Collection<StandardDeviationBodyKinematics> measurements =
26647                 Collections.emptyList();
26648 
26649         final Matrix ba = generateBa();
26650         final double biasX = ba.getElementAtIndex(0);
26651         final double biasY = ba.getElementAtIndex(1);
26652         final double biasZ = ba.getElementAtIndex(2);
26653 
26654         final Matrix ma = generateMaCommonAxis();
26655         final double sx = ma.getElementAt(0, 0);
26656         final double sy = ma.getElementAt(1, 1);
26657         final double sz = ma.getElementAt(2, 2);
26658         final double mxy = ma.getElementAt(0, 1);
26659         final double mxz = ma.getElementAt(0, 2);
26660         final double myx = ma.getElementAt(1, 0);
26661         final double myz = ma.getElementAt(1, 2);
26662         final double mzx = ma.getElementAt(2, 0);
26663         final double mzy = ma.getElementAt(2, 1);
26664 
26665         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26666         final double latitude = Math.toRadians(
26667                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26668         final double longitude = Math.toRadians(
26669                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26670         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26671         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26672         final NEDVelocity nedVelocity = new NEDVelocity();
26673         final ECEFPosition ecefPosition = new ECEFPosition();
26674         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26675         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26676                 ecefPosition, ecefVelocity);
26677         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26678                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26679         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26680 
26681         KnownGravityNormAccelerometerCalibrator calibrator =
26682                 new KnownGravityNormAccelerometerCalibrator(
26683                         gravityNorm, measurements,
26684                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
26685                         myx, myz, mzx, mzy, this);
26686 
26687         // check default values
26688         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26689         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26690         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26691         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26692         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26693         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26694         final Acceleration bx2 = new Acceleration(0.0,
26695                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26696         calibrator.getInitialBiasXAsAcceleration(bx2);
26697         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26698         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26699         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26700         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26701         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26702         final Acceleration by2 = new Acceleration(0.0,
26703                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26704         calibrator.getInitialBiasYAsAcceleration(by2);
26705         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26706         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26707         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26708         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26709         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26710         final Acceleration bz2 = new Acceleration(0.0,
26711                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26712         calibrator.getInitialBiasZAsAcceleration(bz2);
26713         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26714         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26715         assertEquals(calibrator.getInitialSx(), sx, 0.0);
26716         assertEquals(calibrator.getInitialSy(), sy, 0.0);
26717         assertEquals(calibrator.getInitialSz(), sz, 0.0);
26718         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26719         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26720         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26721         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26722         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26723         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26724         final double[] bias1 = calibrator.getInitialBias();
26725         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26726         final double[] bias2 = new double[3];
26727         calibrator.getInitialBias(bias2);
26728         assertArrayEquals(bias1, bias2, 0.0);
26729         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26730         assertEquals(b1, ba);
26731         final Matrix b2 = new Matrix(3, 1);
26732         calibrator.getInitialBiasAsMatrix(b2);
26733         assertEquals(b1, b2);
26734         final Matrix ma1 = new Matrix(3, 3);
26735         ma1.setSubmatrix(0, 0,
26736                 2, 2,
26737                 new double[]{sx, myx, mzx,
26738                         mxy, sy, mzy,
26739                         mxz, myz, sz});
26740         assertEquals(calibrator.getInitialMa(), ma1);
26741         final Matrix ma2 = new Matrix(3, 3);
26742         calibrator.getInitialMa(ma2);
26743         assertEquals(ma1, ma2);
26744         assertSame(calibrator.getMeasurements(), measurements);
26745         assertTrue(calibrator.isCommonAxisUsed());
26746         assertSame(calibrator.getListener(), this);
26747         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26748         assertFalse(calibrator.isReady());
26749         assertFalse(calibrator.isRunning());
26750         assertNull(calibrator.getEstimatedBiases());
26751         assertFalse(calibrator.getEstimatedBiases(null));
26752         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26753         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26754         assertNull(calibrator.getEstimatedBiasFx());
26755         assertNull(calibrator.getEstimatedBiasFy());
26756         assertNull(calibrator.getEstimatedBiasFz());
26757         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26758         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26759         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26760         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26761         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26762         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26763         assertNull(calibrator.getEstimatedMa());
26764         assertNull(calibrator.getEstimatedSx());
26765         assertNull(calibrator.getEstimatedSy());
26766         assertNull(calibrator.getEstimatedSz());
26767         assertNull(calibrator.getEstimatedMxy());
26768         assertNull(calibrator.getEstimatedMxz());
26769         assertNull(calibrator.getEstimatedMyx());
26770         assertNull(calibrator.getEstimatedMyz());
26771         assertNull(calibrator.getEstimatedMzx());
26772         assertNull(calibrator.getEstimatedMzy());
26773         assertNull(calibrator.getEstimatedCovariance());
26774         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26775         assertNotNull(calibrator.getGroundTruthGravityNorm());
26776         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26777         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26778         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26779                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26780         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26781         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26782         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26783 
26784         // Force IllegalArgumentException
26785         final Acceleration invalidGravityNorm = new Acceleration(
26786                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26787 
26788         calibrator = null;
26789         try {
26790             calibrator = new KnownGravityNormAccelerometerCalibrator(
26791                     invalidGravityNorm, measurements,
26792                     true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
26793                     myx, myz, mzx, mzy, this);
26794             fail("IllegalArgumentException expected but not thrown");
26795         } catch (final IllegalArgumentException ignore) {
26796         }
26797         assertNull(calibrator);
26798     }
26799 
26800     @Test
26801     public void testConstructor203() throws WrongSizeException {
26802         final Matrix ba = generateBa();
26803         final double biasX = ba.getElementAtIndex(0);
26804         final double biasY = ba.getElementAtIndex(1);
26805         final double biasZ = ba.getElementAtIndex(2);
26806 
26807         final Acceleration bx = new Acceleration(biasX,
26808                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26809         final Acceleration by = new Acceleration(biasY,
26810                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26811         final Acceleration bz = new Acceleration(biasZ,
26812                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26813 
26814         final Matrix ma = generateMaCommonAxis();
26815         final double sx = ma.getElementAt(0, 0);
26816         final double sy = ma.getElementAt(1, 1);
26817         final double sz = ma.getElementAt(2, 2);
26818         final double mxy = ma.getElementAt(0, 1);
26819         final double mxz = ma.getElementAt(0, 2);
26820         final double myx = ma.getElementAt(1, 0);
26821         final double myz = ma.getElementAt(1, 2);
26822         final double mzx = ma.getElementAt(2, 0);
26823         final double mzy = ma.getElementAt(2, 1);
26824 
26825         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26826         final double latitude = Math.toRadians(
26827                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26828         final double longitude = Math.toRadians(
26829                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26830         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26831         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26832         final NEDVelocity nedVelocity = new NEDVelocity();
26833         final ECEFPosition ecefPosition = new ECEFPosition();
26834         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26835         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26836                 ecefPosition, ecefVelocity);
26837         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26838                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26839         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26840 
26841         KnownGravityNormAccelerometerCalibrator calibrator =
26842                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
26843                         bx, by, bz, sx, sy, sz, mxy, mxz,
26844                         myx, myz, mzx, mzy);
26845 
26846         // check default values
26847         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26848         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26849         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26850         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26851         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26852         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26853         final Acceleration bx2 = new Acceleration(0.0,
26854                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26855         calibrator.getInitialBiasXAsAcceleration(bx2);
26856         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26857         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26858         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26859         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26860         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26861         final Acceleration by2 = new Acceleration(0.0,
26862                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26863         calibrator.getInitialBiasYAsAcceleration(by2);
26864         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26865         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26866         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26867         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26868         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26869         final Acceleration bz2 = new Acceleration(0.0,
26870                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26871         calibrator.getInitialBiasZAsAcceleration(bz2);
26872         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26873         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26874         assertEquals(calibrator.getInitialSx(), sx, 0.0);
26875         assertEquals(calibrator.getInitialSy(), sy, 0.0);
26876         assertEquals(calibrator.getInitialSz(), sz, 0.0);
26877         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26878         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26879         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26880         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26881         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26882         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26883         final double[] bias1 = calibrator.getInitialBias();
26884         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26885         final double[] bias2 = new double[3];
26886         calibrator.getInitialBias(bias2);
26887         assertArrayEquals(bias1, bias2, 0.0);
26888         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26889         assertEquals(b1, ba);
26890         final Matrix b2 = new Matrix(3, 1);
26891         calibrator.getInitialBiasAsMatrix(b2);
26892         assertEquals(b1, b2);
26893         final Matrix ma1 = new Matrix(3, 3);
26894         ma1.setSubmatrix(0, 0,
26895                 2, 2,
26896                 new double[]{sx, myx, mzx,
26897                         mxy, sy, mzy,
26898                         mxz, myz, sz});
26899         assertEquals(calibrator.getInitialMa(), ma1);
26900         final Matrix ma2 = new Matrix(3, 3);
26901         calibrator.getInitialMa(ma2);
26902         assertEquals(ma1, ma2);
26903         assertNull(calibrator.getMeasurements());
26904         assertFalse(calibrator.isCommonAxisUsed());
26905         assertNull(calibrator.getListener());
26906         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26907         assertFalse(calibrator.isReady());
26908         assertFalse(calibrator.isRunning());
26909         assertNull(calibrator.getEstimatedBiases());
26910         assertFalse(calibrator.getEstimatedBiases(null));
26911         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26912         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26913         assertNull(calibrator.getEstimatedBiasFx());
26914         assertNull(calibrator.getEstimatedBiasFy());
26915         assertNull(calibrator.getEstimatedBiasFz());
26916         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26917         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26918         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26919         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26920         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26921         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26922         assertNull(calibrator.getEstimatedMa());
26923         assertNull(calibrator.getEstimatedSx());
26924         assertNull(calibrator.getEstimatedSy());
26925         assertNull(calibrator.getEstimatedSz());
26926         assertNull(calibrator.getEstimatedMxy());
26927         assertNull(calibrator.getEstimatedMxz());
26928         assertNull(calibrator.getEstimatedMyx());
26929         assertNull(calibrator.getEstimatedMyz());
26930         assertNull(calibrator.getEstimatedMzx());
26931         assertNull(calibrator.getEstimatedMzy());
26932         assertNull(calibrator.getEstimatedCovariance());
26933         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26934         assertNotNull(calibrator.getGroundTruthGravityNorm());
26935         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26936         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26937         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26938                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26939         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26940         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26941         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26942 
26943         // Force IllegalArgumentException
26944         final Acceleration invalidGravityNorm = new Acceleration(
26945                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26946 
26947         calibrator = null;
26948         try {
26949             calibrator = new KnownGravityNormAccelerometerCalibrator(
26950                     invalidGravityNorm, bx, by, bz, sx, sy, sz,
26951                     mxy, mxz, myx, myz, mzx, mzy);
26952             fail("IllegalArgumentException expected but not thrown");
26953         } catch (final IllegalArgumentException ignore) {
26954         }
26955         assertNull(calibrator);
26956     }
26957 
26958     @Test
26959     public void testConstructor204() throws WrongSizeException {
26960         final Matrix ba = generateBa();
26961         final double biasX = ba.getElementAtIndex(0);
26962         final double biasY = ba.getElementAtIndex(1);
26963         final double biasZ = ba.getElementAtIndex(2);
26964 
26965         final Acceleration bx = new Acceleration(biasX,
26966                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26967         final Acceleration by = new Acceleration(biasY,
26968                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26969         final Acceleration bz = new Acceleration(biasZ,
26970                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26971 
26972         final Matrix ma = generateMaCommonAxis();
26973         final double sx = ma.getElementAt(0, 0);
26974         final double sy = ma.getElementAt(1, 1);
26975         final double sz = ma.getElementAt(2, 2);
26976         final double mxy = ma.getElementAt(0, 1);
26977         final double mxz = ma.getElementAt(0, 2);
26978         final double myx = ma.getElementAt(1, 0);
26979         final double myz = ma.getElementAt(1, 2);
26980         final double mzx = ma.getElementAt(2, 0);
26981         final double mzy = ma.getElementAt(2, 1);
26982 
26983         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26984         final double latitude = Math.toRadians(
26985                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26986         final double longitude = Math.toRadians(
26987                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26988         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26989         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26990         final NEDVelocity nedVelocity = new NEDVelocity();
26991         final ECEFPosition ecefPosition = new ECEFPosition();
26992         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26993         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26994                 ecefPosition, ecefVelocity);
26995         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26996                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26997         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
26998 
26999         KnownGravityNormAccelerometerCalibrator calibrator =
27000                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
27001                         bx, by, bz, sx, sy, sz, mxy, mxz,
27002                         myx, myz, mzx, mzy, this);
27003 
27004         // check default values
27005         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27006         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27007         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27008         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27009         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27010         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27011         final Acceleration bx2 = new Acceleration(0.0,
27012                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27013         calibrator.getInitialBiasXAsAcceleration(bx2);
27014         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27015         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27016         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27017         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27018         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27019         final Acceleration by2 = new Acceleration(0.0,
27020                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27021         calibrator.getInitialBiasYAsAcceleration(by2);
27022         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27023         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27024         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27025         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27026         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27027         final Acceleration bz2 = new Acceleration(0.0,
27028                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27029         calibrator.getInitialBiasZAsAcceleration(bz2);
27030         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27031         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27032         assertEquals(calibrator.getInitialSx(), sx, 0.0);
27033         assertEquals(calibrator.getInitialSy(), sy, 0.0);
27034         assertEquals(calibrator.getInitialSz(), sz, 0.0);
27035         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27036         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27037         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27038         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27039         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27040         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27041         final double[] bias1 = calibrator.getInitialBias();
27042         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
27043         final double[] bias2 = new double[3];
27044         calibrator.getInitialBias(bias2);
27045         assertArrayEquals(bias1, bias2, 0.0);
27046         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27047         assertEquals(b1, ba);
27048         final Matrix b2 = new Matrix(3, 1);
27049         calibrator.getInitialBiasAsMatrix(b2);
27050         assertEquals(b1, b2);
27051         final Matrix ma1 = new Matrix(3, 3);
27052         ma1.setSubmatrix(0, 0,
27053                 2, 2,
27054                 new double[]{sx, myx, mzx,
27055                         mxy, sy, mzy,
27056                         mxz, myz, sz});
27057         assertEquals(calibrator.getInitialMa(), ma1);
27058         final Matrix ma2 = new Matrix(3, 3);
27059         calibrator.getInitialMa(ma2);
27060         assertEquals(ma1, ma2);
27061         assertNull(calibrator.getMeasurements());
27062         assertFalse(calibrator.isCommonAxisUsed());
27063         assertSame(calibrator.getListener(), this);
27064         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27065         assertFalse(calibrator.isReady());
27066         assertFalse(calibrator.isRunning());
27067         assertNull(calibrator.getEstimatedBiases());
27068         assertFalse(calibrator.getEstimatedBiases(null));
27069         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27070         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27071         assertNull(calibrator.getEstimatedBiasFx());
27072         assertNull(calibrator.getEstimatedBiasFy());
27073         assertNull(calibrator.getEstimatedBiasFz());
27074         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27075         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27076         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27077         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27078         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27079         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27080         assertNull(calibrator.getEstimatedMa());
27081         assertNull(calibrator.getEstimatedSx());
27082         assertNull(calibrator.getEstimatedSy());
27083         assertNull(calibrator.getEstimatedSz());
27084         assertNull(calibrator.getEstimatedMxy());
27085         assertNull(calibrator.getEstimatedMxz());
27086         assertNull(calibrator.getEstimatedMyx());
27087         assertNull(calibrator.getEstimatedMyz());
27088         assertNull(calibrator.getEstimatedMzx());
27089         assertNull(calibrator.getEstimatedMzy());
27090         assertNull(calibrator.getEstimatedCovariance());
27091         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27092         assertNotNull(calibrator.getGroundTruthGravityNorm());
27093         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27094         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27095         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27096                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27097         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27098         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27099         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27100 
27101         // Force IllegalArgumentException
27102         final Acceleration invalidGravityNorm = new Acceleration(
27103                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27104 
27105         calibrator = null;
27106         try {
27107             calibrator = new KnownGravityNormAccelerometerCalibrator(
27108                     invalidGravityNorm, bx, by, bz, sx, sy, sz,
27109                     mxy, mxz, myx, myz, mzx, mzy, this);
27110             fail("IllegalArgumentException expected but not thrown");
27111         } catch (final IllegalArgumentException ignore) {
27112         }
27113         assertNull(calibrator);
27114     }
27115 
27116     @Test
27117     public void testConstructor205() throws WrongSizeException {
27118         final Collection<StandardDeviationBodyKinematics> measurements =
27119                 Collections.emptyList();
27120 
27121         final Matrix ba = generateBa();
27122         final double biasX = ba.getElementAtIndex(0);
27123         final double biasY = ba.getElementAtIndex(1);
27124         final double biasZ = ba.getElementAtIndex(2);
27125 
27126         final Acceleration bx = new Acceleration(biasX,
27127                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27128         final Acceleration by = new Acceleration(biasY,
27129                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27130         final Acceleration bz = new Acceleration(biasZ,
27131                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27132 
27133         final Matrix ma = generateMaCommonAxis();
27134         final double sx = ma.getElementAt(0, 0);
27135         final double sy = ma.getElementAt(1, 1);
27136         final double sz = ma.getElementAt(2, 2);
27137         final double mxy = ma.getElementAt(0, 1);
27138         final double mxz = ma.getElementAt(0, 2);
27139         final double myx = ma.getElementAt(1, 0);
27140         final double myz = ma.getElementAt(1, 2);
27141         final double mzx = ma.getElementAt(2, 0);
27142         final double mzy = ma.getElementAt(2, 1);
27143 
27144         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27145         final double latitude = Math.toRadians(
27146                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27147         final double longitude = Math.toRadians(
27148                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27149         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27150         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27151         final NEDVelocity nedVelocity = new NEDVelocity();
27152         final ECEFPosition ecefPosition = new ECEFPosition();
27153         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27154         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27155                 ecefPosition, ecefVelocity);
27156         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27157                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27158         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27159 
27160         KnownGravityNormAccelerometerCalibrator calibrator =
27161                 new KnownGravityNormAccelerometerCalibrator(
27162                         gravityNorm, measurements,
27163                         bx, by, bz, sx, sy, sz, mxy, mxz,
27164                         myx, myz, mzx, mzy);
27165 
27166         // check default values
27167         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27168         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27169         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27170         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27171         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27172         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27173         final Acceleration bx2 = new Acceleration(0.0,
27174                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27175         calibrator.getInitialBiasXAsAcceleration(bx2);
27176         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27177         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27178         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27179         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27180         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27181         final Acceleration by2 = new Acceleration(0.0,
27182                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27183         calibrator.getInitialBiasYAsAcceleration(by2);
27184         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27185         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27186         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27187         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27188         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27189         final Acceleration bz2 = new Acceleration(0.0,
27190                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27191         calibrator.getInitialBiasZAsAcceleration(bz2);
27192         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27193         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27194         assertEquals(calibrator.getInitialSx(), sx, 0.0);
27195         assertEquals(calibrator.getInitialSy(), sy, 0.0);
27196         assertEquals(calibrator.getInitialSz(), sz, 0.0);
27197         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27198         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27199         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27200         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27201         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27202         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27203         final double[] bias1 = calibrator.getInitialBias();
27204         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
27205         final double[] bias2 = new double[3];
27206         calibrator.getInitialBias(bias2);
27207         assertArrayEquals(bias1, bias2, 0.0);
27208         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27209         assertEquals(b1, ba);
27210         final Matrix b2 = new Matrix(3, 1);
27211         calibrator.getInitialBiasAsMatrix(b2);
27212         assertEquals(b1, b2);
27213         final Matrix ma1 = new Matrix(3, 3);
27214         ma1.setSubmatrix(0, 0,
27215                 2, 2,
27216                 new double[]{sx, myx, mzx,
27217                         mxy, sy, mzy,
27218                         mxz, myz, sz});
27219         assertEquals(calibrator.getInitialMa(), ma1);
27220         final Matrix ma2 = new Matrix(3, 3);
27221         calibrator.getInitialMa(ma2);
27222         assertEquals(ma1, ma2);
27223         assertSame(calibrator.getMeasurements(), measurements);
27224         assertFalse(calibrator.isCommonAxisUsed());
27225         assertNull(calibrator.getListener());
27226         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27227         assertFalse(calibrator.isReady());
27228         assertFalse(calibrator.isRunning());
27229         assertNull(calibrator.getEstimatedBiases());
27230         assertFalse(calibrator.getEstimatedBiases(null));
27231         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27232         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27233         assertNull(calibrator.getEstimatedBiasFx());
27234         assertNull(calibrator.getEstimatedBiasFy());
27235         assertNull(calibrator.getEstimatedBiasFz());
27236         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27237         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27238         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27239         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27240         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27241         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27242         assertNull(calibrator.getEstimatedMa());
27243         assertNull(calibrator.getEstimatedSx());
27244         assertNull(calibrator.getEstimatedSy());
27245         assertNull(calibrator.getEstimatedSz());
27246         assertNull(calibrator.getEstimatedMxy());
27247         assertNull(calibrator.getEstimatedMxz());
27248         assertNull(calibrator.getEstimatedMyx());
27249         assertNull(calibrator.getEstimatedMyz());
27250         assertNull(calibrator.getEstimatedMzx());
27251         assertNull(calibrator.getEstimatedMzy());
27252         assertNull(calibrator.getEstimatedCovariance());
27253         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27254         assertNotNull(calibrator.getGroundTruthGravityNorm());
27255         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27256         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27257         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27258                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27259         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27260         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27261         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27262 
27263         // Force IllegalArgumentException
27264         final Acceleration invalidGravityNorm = new Acceleration(
27265                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27266 
27267         calibrator = null;
27268         try {
27269             calibrator = new KnownGravityNormAccelerometerCalibrator(
27270                     invalidGravityNorm, measurements,
27271                     bx, by, bz, sx, sy, sz, mxy, mxz,
27272                     myx, myz, mzx, mzy);
27273             fail("IllegalArgumentException expected but not thrown");
27274         } catch (final IllegalArgumentException ignore) {
27275         }
27276         assertNull(calibrator);
27277     }
27278 
27279     @Test
27280     public void testConstructor206() throws WrongSizeException {
27281         final Collection<StandardDeviationBodyKinematics> measurements =
27282                 Collections.emptyList();
27283 
27284         final Matrix ba = generateBa();
27285         final double biasX = ba.getElementAtIndex(0);
27286         final double biasY = ba.getElementAtIndex(1);
27287         final double biasZ = ba.getElementAtIndex(2);
27288 
27289         final Acceleration bx = new Acceleration(biasX,
27290                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27291         final Acceleration by = new Acceleration(biasY,
27292                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27293         final Acceleration bz = new Acceleration(biasZ,
27294                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27295 
27296         final Matrix ma = generateMaCommonAxis();
27297         final double sx = ma.getElementAt(0, 0);
27298         final double sy = ma.getElementAt(1, 1);
27299         final double sz = ma.getElementAt(2, 2);
27300         final double mxy = ma.getElementAt(0, 1);
27301         final double mxz = ma.getElementAt(0, 2);
27302         final double myx = ma.getElementAt(1, 0);
27303         final double myz = ma.getElementAt(1, 2);
27304         final double mzx = ma.getElementAt(2, 0);
27305         final double mzy = ma.getElementAt(2, 1);
27306 
27307         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27308         final double latitude = Math.toRadians(
27309                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27310         final double longitude = Math.toRadians(
27311                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27312         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27313         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27314         final NEDVelocity nedVelocity = new NEDVelocity();
27315         final ECEFPosition ecefPosition = new ECEFPosition();
27316         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27317         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27318                 ecefPosition, ecefVelocity);
27319         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27320                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27321         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27322 
27323         KnownGravityNormAccelerometerCalibrator calibrator =
27324                 new KnownGravityNormAccelerometerCalibrator(
27325                         gravityNorm, measurements,
27326                         bx, by, bz, sx, sy, sz, mxy, mxz,
27327                         myx, myz, mzx, mzy, this);
27328 
27329         // check default values
27330         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27331         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27332         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27333         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27334         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27335         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27336         final Acceleration bx2 = new Acceleration(0.0,
27337                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27338         calibrator.getInitialBiasXAsAcceleration(bx2);
27339         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27340         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27341         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27342         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27343         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27344         final Acceleration by2 = new Acceleration(0.0,
27345                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27346         calibrator.getInitialBiasYAsAcceleration(by2);
27347         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27348         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27349         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27350         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27351         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27352         final Acceleration bz2 = new Acceleration(0.0,
27353                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27354         calibrator.getInitialBiasZAsAcceleration(bz2);
27355         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27356         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27357         assertEquals(calibrator.getInitialSx(), sx, 0.0);
27358         assertEquals(calibrator.getInitialSy(), sy, 0.0);
27359         assertEquals(calibrator.getInitialSz(), sz, 0.0);
27360         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27361         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27362         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27363         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27364         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27365         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27366         final double[] bias1 = calibrator.getInitialBias();
27367         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
27368         final double[] bias2 = new double[3];
27369         calibrator.getInitialBias(bias2);
27370         assertArrayEquals(bias1, bias2, 0.0);
27371         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27372         assertEquals(b1, ba);
27373         final Matrix b2 = new Matrix(3, 1);
27374         calibrator.getInitialBiasAsMatrix(b2);
27375         assertEquals(b1, b2);
27376         final Matrix ma1 = new Matrix(3, 3);
27377         ma1.setSubmatrix(0, 0,
27378                 2, 2,
27379                 new double[]{sx, myx, mzx,
27380                         mxy, sy, mzy,
27381                         mxz, myz, sz});
27382         assertEquals(calibrator.getInitialMa(), ma1);
27383         final Matrix ma2 = new Matrix(3, 3);
27384         calibrator.getInitialMa(ma2);
27385         assertEquals(ma1, ma2);
27386         assertSame(calibrator.getMeasurements(), measurements);
27387         assertFalse(calibrator.isCommonAxisUsed());
27388         assertSame(calibrator.getListener(), this);
27389         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27390         assertFalse(calibrator.isReady());
27391         assertFalse(calibrator.isRunning());
27392         assertNull(calibrator.getEstimatedBiases());
27393         assertFalse(calibrator.getEstimatedBiases(null));
27394         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27395         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27396         assertNull(calibrator.getEstimatedBiasFx());
27397         assertNull(calibrator.getEstimatedBiasFy());
27398         assertNull(calibrator.getEstimatedBiasFz());
27399         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27400         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27401         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27402         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27403         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27404         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27405         assertNull(calibrator.getEstimatedMa());
27406         assertNull(calibrator.getEstimatedSx());
27407         assertNull(calibrator.getEstimatedSy());
27408         assertNull(calibrator.getEstimatedSz());
27409         assertNull(calibrator.getEstimatedMxy());
27410         assertNull(calibrator.getEstimatedMxz());
27411         assertNull(calibrator.getEstimatedMyx());
27412         assertNull(calibrator.getEstimatedMyz());
27413         assertNull(calibrator.getEstimatedMzx());
27414         assertNull(calibrator.getEstimatedMzy());
27415         assertNull(calibrator.getEstimatedCovariance());
27416         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27417         assertNotNull(calibrator.getGroundTruthGravityNorm());
27418         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27419         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27420         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27421                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27422         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27423         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27424         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27425 
27426         // Force IllegalArgumentException
27427         final Acceleration invalidGravityNorm = new Acceleration(
27428                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27429 
27430         calibrator = null;
27431         try {
27432             calibrator = new KnownGravityNormAccelerometerCalibrator(
27433                     invalidGravityNorm, measurements,
27434                     bx, by, bz, sx, sy, sz, mxy, mxz,
27435                     myx, myz, mzx, mzy, this);
27436             fail("IllegalArgumentException expected but not thrown");
27437         } catch (final IllegalArgumentException ignore) {
27438         }
27439         assertNull(calibrator);
27440     }
27441 
27442     @Test
27443     public void testConstructor207() throws WrongSizeException {
27444         final Matrix ba = generateBa();
27445         final double biasX = ba.getElementAtIndex(0);
27446         final double biasY = ba.getElementAtIndex(1);
27447         final double biasZ = ba.getElementAtIndex(2);
27448 
27449         final Acceleration bx = new Acceleration(biasX,
27450                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27451         final Acceleration by = new Acceleration(biasY,
27452                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27453         final Acceleration bz = new Acceleration(biasZ,
27454                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27455 
27456         final Matrix ma = generateMaCommonAxis();
27457         final double sx = ma.getElementAt(0, 0);
27458         final double sy = ma.getElementAt(1, 1);
27459         final double sz = ma.getElementAt(2, 2);
27460         final double mxy = ma.getElementAt(0, 1);
27461         final double mxz = ma.getElementAt(0, 2);
27462         final double myx = ma.getElementAt(1, 0);
27463         final double myz = ma.getElementAt(1, 2);
27464         final double mzx = ma.getElementAt(2, 0);
27465         final double mzy = ma.getElementAt(2, 1);
27466 
27467         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27468         final double latitude = Math.toRadians(
27469                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27470         final double longitude = Math.toRadians(
27471                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27472         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27473         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27474         final NEDVelocity nedVelocity = new NEDVelocity();
27475         final ECEFPosition ecefPosition = new ECEFPosition();
27476         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27477         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27478                 ecefPosition, ecefVelocity);
27479         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27480                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27481         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27482 
27483         KnownGravityNormAccelerometerCalibrator calibrator =
27484                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
27485                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
27486                         myx, myz, mzx, mzy);
27487 
27488         // check default values
27489         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27490         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27491         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27492         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27493         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27494         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27495         final Acceleration bx2 = new Acceleration(0.0,
27496                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27497         calibrator.getInitialBiasXAsAcceleration(bx2);
27498         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27499         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27500         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27501         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27502         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27503         final Acceleration by2 = new Acceleration(0.0,
27504                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27505         calibrator.getInitialBiasYAsAcceleration(by2);
27506         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27507         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27508         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27509         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27510         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27511         final Acceleration bz2 = new Acceleration(0.0,
27512                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27513         calibrator.getInitialBiasZAsAcceleration(bz2);
27514         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27515         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27516         assertEquals(calibrator.getInitialSx(), sx, 0.0);
27517         assertEquals(calibrator.getInitialSy(), sy, 0.0);
27518         assertEquals(calibrator.getInitialSz(), sz, 0.0);
27519         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27520         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27521         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27522         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27523         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27524         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27525         final double[] bias1 = calibrator.getInitialBias();
27526         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
27527         final double[] bias2 = new double[3];
27528         calibrator.getInitialBias(bias2);
27529         assertArrayEquals(bias1, bias2, 0.0);
27530         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27531         assertEquals(b1, ba);
27532         final Matrix b2 = new Matrix(3, 1);
27533         calibrator.getInitialBiasAsMatrix(b2);
27534         assertEquals(b1, b2);
27535         final Matrix ma1 = new Matrix(3, 3);
27536         ma1.setSubmatrix(0, 0,
27537                 2, 2,
27538                 new double[]{sx, myx, mzx,
27539                         mxy, sy, mzy,
27540                         mxz, myz, sz});
27541         assertEquals(calibrator.getInitialMa(), ma1);
27542         final Matrix ma2 = new Matrix(3, 3);
27543         calibrator.getInitialMa(ma2);
27544         assertEquals(ma1, ma2);
27545         assertNull(calibrator.getMeasurements());
27546         assertTrue(calibrator.isCommonAxisUsed());
27547         assertNull(calibrator.getListener());
27548         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27549         assertFalse(calibrator.isReady());
27550         assertFalse(calibrator.isRunning());
27551         assertNull(calibrator.getEstimatedBiases());
27552         assertFalse(calibrator.getEstimatedBiases(null));
27553         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27554         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27555         assertNull(calibrator.getEstimatedBiasFx());
27556         assertNull(calibrator.getEstimatedBiasFy());
27557         assertNull(calibrator.getEstimatedBiasFz());
27558         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27559         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27560         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27561         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27562         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27563         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27564         assertNull(calibrator.getEstimatedMa());
27565         assertNull(calibrator.getEstimatedSx());
27566         assertNull(calibrator.getEstimatedSy());
27567         assertNull(calibrator.getEstimatedSz());
27568         assertNull(calibrator.getEstimatedMxy());
27569         assertNull(calibrator.getEstimatedMxz());
27570         assertNull(calibrator.getEstimatedMyx());
27571         assertNull(calibrator.getEstimatedMyz());
27572         assertNull(calibrator.getEstimatedMzx());
27573         assertNull(calibrator.getEstimatedMzy());
27574         assertNull(calibrator.getEstimatedCovariance());
27575         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27576         assertNotNull(calibrator.getGroundTruthGravityNorm());
27577         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27578         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27579         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27580                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27581         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27582         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27583         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27584 
27585         // Force IllegalArgumentException
27586         final Acceleration invalidGravityNorm = new Acceleration(
27587                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27588 
27589         calibrator = null;
27590         try {
27591             calibrator = new KnownGravityNormAccelerometerCalibrator(
27592                     invalidGravityNorm, true,
27593                     bx, by, bz, sx, sy, sz, mxy, mxz,
27594                     myx, myz, mzx, mzy);
27595             fail("IllegalArgumentException expected but not thrown");
27596         } catch (final IllegalArgumentException ignore) {
27597         }
27598         assertNull(calibrator);
27599     }
27600 
27601     @Test
27602     public void testConstructor208() throws WrongSizeException {
27603         final Matrix ba = generateBa();
27604         final double biasX = ba.getElementAtIndex(0);
27605         final double biasY = ba.getElementAtIndex(1);
27606         final double biasZ = ba.getElementAtIndex(2);
27607 
27608         final Acceleration bx = new Acceleration(biasX,
27609                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27610         final Acceleration by = new Acceleration(biasY,
27611                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27612         final Acceleration bz = new Acceleration(biasZ,
27613                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27614 
27615         final Matrix ma = generateMaCommonAxis();
27616         final double sx = ma.getElementAt(0, 0);
27617         final double sy = ma.getElementAt(1, 1);
27618         final double sz = ma.getElementAt(2, 2);
27619         final double mxy = ma.getElementAt(0, 1);
27620         final double mxz = ma.getElementAt(0, 2);
27621         final double myx = ma.getElementAt(1, 0);
27622         final double myz = ma.getElementAt(1, 2);
27623         final double mzx = ma.getElementAt(2, 0);
27624         final double mzy = ma.getElementAt(2, 1);
27625 
27626         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27627         final double latitude = Math.toRadians(
27628                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27629         final double longitude = Math.toRadians(
27630                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27631         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27632         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27633         final NEDVelocity nedVelocity = new NEDVelocity();
27634         final ECEFPosition ecefPosition = new ECEFPosition();
27635         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27636         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27637                 ecefPosition, ecefVelocity);
27638         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27639                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27640         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27641 
27642         KnownGravityNormAccelerometerCalibrator calibrator =
27643                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
27644                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
27645                         myx, myz, mzx, mzy, this);
27646 
27647         // check default values
27648         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27649         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27650         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27651         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27652         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27653         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27654         final Acceleration bx2 = new Acceleration(0.0,
27655                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27656         calibrator.getInitialBiasXAsAcceleration(bx2);
27657         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27658         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27659         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27660         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27661         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27662         final Acceleration by2 = new Acceleration(0.0,
27663                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27664         calibrator.getInitialBiasYAsAcceleration(by2);
27665         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27666         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27667         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27668         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27669         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27670         final Acceleration bz2 = new Acceleration(0.0,
27671                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27672         calibrator.getInitialBiasZAsAcceleration(bz2);
27673         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27674         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27675         assertEquals(calibrator.getInitialSx(), sx, 0.0);
27676         assertEquals(calibrator.getInitialSy(), sy, 0.0);
27677         assertEquals(calibrator.getInitialSz(), sz, 0.0);
27678         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27679         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27680         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27681         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27682         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27683         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27684         final double[] bias1 = calibrator.getInitialBias();
27685         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
27686         final double[] bias2 = new double[3];
27687         calibrator.getInitialBias(bias2);
27688         assertArrayEquals(bias1, bias2, 0.0);
27689         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27690         assertEquals(b1, ba);
27691         final Matrix b2 = new Matrix(3, 1);
27692         calibrator.getInitialBiasAsMatrix(b2);
27693         assertEquals(b1, b2);
27694         final Matrix ma1 = new Matrix(3, 3);
27695         ma1.setSubmatrix(0, 0,
27696                 2, 2,
27697                 new double[]{sx, myx, mzx,
27698                         mxy, sy, mzy,
27699                         mxz, myz, sz});
27700         assertEquals(calibrator.getInitialMa(), ma1);
27701         final Matrix ma2 = new Matrix(3, 3);
27702         calibrator.getInitialMa(ma2);
27703         assertEquals(ma1, ma2);
27704         assertNull(calibrator.getMeasurements());
27705         assertTrue(calibrator.isCommonAxisUsed());
27706         assertSame(calibrator.getListener(), this);
27707         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27708         assertFalse(calibrator.isReady());
27709         assertFalse(calibrator.isRunning());
27710         assertNull(calibrator.getEstimatedBiases());
27711         assertFalse(calibrator.getEstimatedBiases(null));
27712         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27713         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27714         assertNull(calibrator.getEstimatedBiasFx());
27715         assertNull(calibrator.getEstimatedBiasFy());
27716         assertNull(calibrator.getEstimatedBiasFz());
27717         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27718         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27719         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27720         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27721         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27722         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27723         assertNull(calibrator.getEstimatedMa());
27724         assertNull(calibrator.getEstimatedSx());
27725         assertNull(calibrator.getEstimatedSy());
27726         assertNull(calibrator.getEstimatedSz());
27727         assertNull(calibrator.getEstimatedMxy());
27728         assertNull(calibrator.getEstimatedMxz());
27729         assertNull(calibrator.getEstimatedMyx());
27730         assertNull(calibrator.getEstimatedMyz());
27731         assertNull(calibrator.getEstimatedMzx());
27732         assertNull(calibrator.getEstimatedMzy());
27733         assertNull(calibrator.getEstimatedCovariance());
27734         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27735         assertNotNull(calibrator.getGroundTruthGravityNorm());
27736         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27737         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27738         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27739                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27740         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27741         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27742         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27743 
27744         // Force IllegalArgumentException
27745         final Acceleration invalidGravityNorm = new Acceleration(
27746                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27747 
27748         calibrator = null;
27749         try {
27750             calibrator = new KnownGravityNormAccelerometerCalibrator(
27751                     invalidGravityNorm, true,
27752                     bx, by, bz, sx, sy, sz, mxy, mxz,
27753                     myx, myz, mzx, mzy, this);
27754             fail("IllegalArgumentException expected but not thrown");
27755         } catch (final IllegalArgumentException ignore) {
27756         }
27757         assertNull(calibrator);
27758     }
27759 
27760     @Test
27761     public void testConstructor209() throws WrongSizeException {
27762         final Collection<StandardDeviationBodyKinematics> measurements =
27763                 Collections.emptyList();
27764 
27765         final Matrix ba = generateBa();
27766         final double biasX = ba.getElementAtIndex(0);
27767         final double biasY = ba.getElementAtIndex(1);
27768         final double biasZ = ba.getElementAtIndex(2);
27769 
27770         final Acceleration bx = new Acceleration(biasX,
27771                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27772         final Acceleration by = new Acceleration(biasY,
27773                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27774         final Acceleration bz = new Acceleration(biasZ,
27775                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27776 
27777         final Matrix ma = generateMaCommonAxis();
27778         final double sx = ma.getElementAt(0, 0);
27779         final double sy = ma.getElementAt(1, 1);
27780         final double sz = ma.getElementAt(2, 2);
27781         final double mxy = ma.getElementAt(0, 1);
27782         final double mxz = ma.getElementAt(0, 2);
27783         final double myx = ma.getElementAt(1, 0);
27784         final double myz = ma.getElementAt(1, 2);
27785         final double mzx = ma.getElementAt(2, 0);
27786         final double mzy = ma.getElementAt(2, 1);
27787 
27788         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27789         final double latitude = Math.toRadians(
27790                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27791         final double longitude = Math.toRadians(
27792                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27793         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27794         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27795         final NEDVelocity nedVelocity = new NEDVelocity();
27796         final ECEFPosition ecefPosition = new ECEFPosition();
27797         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27798         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27799                 ecefPosition, ecefVelocity);
27800         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27801                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27802         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27803 
27804         KnownGravityNormAccelerometerCalibrator calibrator =
27805                 new KnownGravityNormAccelerometerCalibrator(
27806                         gravityNorm, measurements,
27807                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
27808                         myx, myz, mzx, mzy);
27809 
27810         // check default values
27811         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27812         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27813         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27814         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27815         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27816         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27817         final Acceleration bx2 = new Acceleration(0.0,
27818                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27819         calibrator.getInitialBiasXAsAcceleration(bx2);
27820         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27821         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27822         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27823         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27824         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27825         final Acceleration by2 = new Acceleration(0.0,
27826                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27827         calibrator.getInitialBiasYAsAcceleration(by2);
27828         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27829         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27830         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27831         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27832         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27833         final Acceleration bz2 = new Acceleration(0.0,
27834                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27835         calibrator.getInitialBiasZAsAcceleration(bz2);
27836         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27837         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27838         assertEquals(calibrator.getInitialSx(), sx, 0.0);
27839         assertEquals(calibrator.getInitialSy(), sy, 0.0);
27840         assertEquals(calibrator.getInitialSz(), sz, 0.0);
27841         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27842         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27843         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27844         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27845         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27846         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27847         final double[] bias1 = calibrator.getInitialBias();
27848         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
27849         final double[] bias2 = new double[3];
27850         calibrator.getInitialBias(bias2);
27851         assertArrayEquals(bias1, bias2, 0.0);
27852         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27853         assertEquals(b1, ba);
27854         final Matrix b2 = new Matrix(3, 1);
27855         calibrator.getInitialBiasAsMatrix(b2);
27856         assertEquals(b1, b2);
27857         final Matrix ma1 = new Matrix(3, 3);
27858         ma1.setSubmatrix(0, 0,
27859                 2, 2,
27860                 new double[]{sx, myx, mzx,
27861                         mxy, sy, mzy,
27862                         mxz, myz, sz});
27863         assertEquals(calibrator.getInitialMa(), ma1);
27864         final Matrix ma2 = new Matrix(3, 3);
27865         calibrator.getInitialMa(ma2);
27866         assertEquals(ma1, ma2);
27867         assertSame(calibrator.getMeasurements(), measurements);
27868         assertTrue(calibrator.isCommonAxisUsed());
27869         assertNull(calibrator.getListener());
27870         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27871         assertFalse(calibrator.isReady());
27872         assertFalse(calibrator.isRunning());
27873         assertNull(calibrator.getEstimatedBiases());
27874         assertFalse(calibrator.getEstimatedBiases(null));
27875         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27876         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27877         assertNull(calibrator.getEstimatedBiasFx());
27878         assertNull(calibrator.getEstimatedBiasFy());
27879         assertNull(calibrator.getEstimatedBiasFz());
27880         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27881         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27882         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27883         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27884         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27885         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27886         assertNull(calibrator.getEstimatedMa());
27887         assertNull(calibrator.getEstimatedSx());
27888         assertNull(calibrator.getEstimatedSy());
27889         assertNull(calibrator.getEstimatedSz());
27890         assertNull(calibrator.getEstimatedMxy());
27891         assertNull(calibrator.getEstimatedMxz());
27892         assertNull(calibrator.getEstimatedMyx());
27893         assertNull(calibrator.getEstimatedMyz());
27894         assertNull(calibrator.getEstimatedMzx());
27895         assertNull(calibrator.getEstimatedMzy());
27896         assertNull(calibrator.getEstimatedCovariance());
27897         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27898         assertNotNull(calibrator.getGroundTruthGravityNorm());
27899         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27900         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27901         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27902                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27903         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27904         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27905         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27906 
27907         // Force IllegalArgumentException
27908         final Acceleration invalidGravityNorm = new Acceleration(
27909                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27910 
27911         calibrator = null;
27912         try {
27913             calibrator = new KnownGravityNormAccelerometerCalibrator(
27914                     invalidGravityNorm, measurements,
27915                     true, bx, by, bz, sx, sy, sz, mxy, mxz,
27916                     myx, myz, mzx, mzy);
27917             fail("IllegalArgumentException expected but not thrown");
27918         } catch (final IllegalArgumentException ignore) {
27919         }
27920         assertNull(calibrator);
27921     }
27922 
27923     @Test
27924     public void testConstructor210() throws WrongSizeException {
27925         final Collection<StandardDeviationBodyKinematics> measurements =
27926                 Collections.emptyList();
27927 
27928         final Matrix ba = generateBa();
27929         final double biasX = ba.getElementAtIndex(0);
27930         final double biasY = ba.getElementAtIndex(1);
27931         final double biasZ = ba.getElementAtIndex(2);
27932 
27933         final Acceleration bx = new Acceleration(biasX,
27934                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27935         final Acceleration by = new Acceleration(biasY,
27936                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27937         final Acceleration bz = new Acceleration(biasZ,
27938                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27939 
27940         final Matrix ma = generateMaCommonAxis();
27941         final double sx = ma.getElementAt(0, 0);
27942         final double sy = ma.getElementAt(1, 1);
27943         final double sz = ma.getElementAt(2, 2);
27944         final double mxy = ma.getElementAt(0, 1);
27945         final double mxz = ma.getElementAt(0, 2);
27946         final double myx = ma.getElementAt(1, 0);
27947         final double myz = ma.getElementAt(1, 2);
27948         final double mzx = ma.getElementAt(2, 0);
27949         final double mzy = ma.getElementAt(2, 1);
27950 
27951         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27952         final double latitude = Math.toRadians(
27953                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27954         final double longitude = Math.toRadians(
27955                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27956         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27957         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27958         final NEDVelocity nedVelocity = new NEDVelocity();
27959         final ECEFPosition ecefPosition = new ECEFPosition();
27960         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27961         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27962                 ecefPosition, ecefVelocity);
27963         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27964                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27965         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
27966 
27967         KnownGravityNormAccelerometerCalibrator calibrator =
27968                 new KnownGravityNormAccelerometerCalibrator(
27969                         gravityNorm, measurements,
27970                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
27971                         myx, myz, mzx, mzy, this);
27972 
27973         // check default values
27974         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27975         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27976         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27977         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27978         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27979         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27980         final Acceleration bx2 = new Acceleration(0.0,
27981                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27982         calibrator.getInitialBiasXAsAcceleration(bx2);
27983         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27984         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27985         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27986         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27987         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27988         final Acceleration by2 = new Acceleration(0.0,
27989                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27990         calibrator.getInitialBiasYAsAcceleration(by2);
27991         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27992         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27993         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27994         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27995         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27996         final Acceleration bz2 = new Acceleration(0.0,
27997                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27998         calibrator.getInitialBiasZAsAcceleration(bz2);
27999         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28000         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28001         assertEquals(calibrator.getInitialSx(), sx, 0.0);
28002         assertEquals(calibrator.getInitialSy(), sy, 0.0);
28003         assertEquals(calibrator.getInitialSz(), sz, 0.0);
28004         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
28005         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28006         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
28007         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
28008         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
28009         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
28010         final double[] bias1 = calibrator.getInitialBias();
28011         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
28012         final double[] bias2 = new double[3];
28013         calibrator.getInitialBias(bias2);
28014         assertArrayEquals(bias1, bias2, 0.0);
28015         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28016         assertEquals(b1, ba);
28017         final Matrix b2 = new Matrix(3, 1);
28018         calibrator.getInitialBiasAsMatrix(b2);
28019         assertEquals(b1, b2);
28020         final Matrix ma1 = new Matrix(3, 3);
28021         ma1.setSubmatrix(0, 0,
28022                 2, 2,
28023                 new double[]{sx, myx, mzx,
28024                         mxy, sy, mzy,
28025                         mxz, myz, sz});
28026         assertEquals(calibrator.getInitialMa(), ma1);
28027         final Matrix ma2 = new Matrix(3, 3);
28028         calibrator.getInitialMa(ma2);
28029         assertEquals(ma1, ma2);
28030         assertSame(calibrator.getMeasurements(), measurements);
28031         assertTrue(calibrator.isCommonAxisUsed());
28032         assertSame(calibrator.getListener(), this);
28033         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28034         assertFalse(calibrator.isReady());
28035         assertFalse(calibrator.isRunning());
28036         assertNull(calibrator.getEstimatedBiases());
28037         assertFalse(calibrator.getEstimatedBiases(null));
28038         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28039         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28040         assertNull(calibrator.getEstimatedBiasFx());
28041         assertNull(calibrator.getEstimatedBiasFy());
28042         assertNull(calibrator.getEstimatedBiasFz());
28043         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28044         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28045         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28046         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28047         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28048         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28049         assertNull(calibrator.getEstimatedMa());
28050         assertNull(calibrator.getEstimatedSx());
28051         assertNull(calibrator.getEstimatedSy());
28052         assertNull(calibrator.getEstimatedSz());
28053         assertNull(calibrator.getEstimatedMxy());
28054         assertNull(calibrator.getEstimatedMxz());
28055         assertNull(calibrator.getEstimatedMyx());
28056         assertNull(calibrator.getEstimatedMyz());
28057         assertNull(calibrator.getEstimatedMzx());
28058         assertNull(calibrator.getEstimatedMzy());
28059         assertNull(calibrator.getEstimatedCovariance());
28060         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28061         assertNotNull(calibrator.getGroundTruthGravityNorm());
28062         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28063         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28064         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28065                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28066         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28067         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28068         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28069 
28070         // Force IllegalArgumentException
28071         final Acceleration invalidGravityNorm = new Acceleration(
28072                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28073 
28074         calibrator = null;
28075         try {
28076             calibrator = new KnownGravityNormAccelerometerCalibrator(
28077                     invalidGravityNorm, measurements,
28078                     true, bx, by, bz, sx, sy, sz, mxy, mxz,
28079                     myx, myz, mzx, mzy, this);
28080             fail("IllegalArgumentException expected but not thrown");
28081         } catch (final IllegalArgumentException ignore) {
28082         }
28083         assertNull(calibrator);
28084     }
28085 
28086     @Test
28087     public void testConstructor211() throws WrongSizeException {
28088         final Matrix ba = generateBa();
28089         final double[] bias = ba.getBuffer();
28090         final double biasX = ba.getElementAtIndex(0);
28091         final double biasY = ba.getElementAtIndex(1);
28092         final double biasZ = ba.getElementAtIndex(2);
28093 
28094         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28095         final double latitude = Math.toRadians(
28096                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28097         final double longitude = Math.toRadians(
28098                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28099         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28100         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28101         final NEDVelocity nedVelocity = new NEDVelocity();
28102         final ECEFPosition ecefPosition = new ECEFPosition();
28103         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28104         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28105                 ecefPosition, ecefVelocity);
28106         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28107                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28108         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28109 
28110         KnownGravityNormAccelerometerCalibrator calibrator =
28111                 new KnownGravityNormAccelerometerCalibrator(
28112                         gravityNorm, bias);
28113 
28114         // check default values
28115         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28116         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28117         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28118         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28119         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28120         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28121         final Acceleration bx2 = new Acceleration(0.0,
28122                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28123         calibrator.getInitialBiasXAsAcceleration(bx2);
28124         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28125         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28126         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28127         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28128         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28129         final Acceleration by2 = new Acceleration(0.0,
28130                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28131         calibrator.getInitialBiasYAsAcceleration(by2);
28132         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28133         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28134         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28135         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28136         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28137         final Acceleration bz2 = new Acceleration(0.0,
28138                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28139         calibrator.getInitialBiasZAsAcceleration(bz2);
28140         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28141         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28142         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28143         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28144         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28145         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28146         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28147         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28148         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28149         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28150         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28151         final double[] bias1 = calibrator.getInitialBias();
28152         assertArrayEquals(bias1, bias, 0.0);
28153         final double[] bias2 = new double[3];
28154         calibrator.getInitialBias(bias2);
28155         assertArrayEquals(bias1, bias2, 0.0);
28156         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28157         assertEquals(b1, ba);
28158         final Matrix b2 = new Matrix(3, 1);
28159         calibrator.getInitialBiasAsMatrix(b2);
28160         assertEquals(b1, b2);
28161         final Matrix ma1 = calibrator.getInitialMa();
28162         assertEquals(ma1, new Matrix(3, 3));
28163         final Matrix ma2 = new Matrix(3, 3);
28164         calibrator.getInitialMa(ma2);
28165         assertEquals(ma1, ma2);
28166         assertNull(calibrator.getMeasurements());
28167         assertFalse(calibrator.isCommonAxisUsed());
28168         assertNull(calibrator.getListener());
28169         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28170         assertFalse(calibrator.isReady());
28171         assertFalse(calibrator.isRunning());
28172         assertNull(calibrator.getEstimatedBiases());
28173         assertFalse(calibrator.getEstimatedBiases(null));
28174         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28175         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28176         assertNull(calibrator.getEstimatedBiasFx());
28177         assertNull(calibrator.getEstimatedBiasFy());
28178         assertNull(calibrator.getEstimatedBiasFz());
28179         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28180         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28181         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28182         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28183         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28184         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28185         assertNull(calibrator.getEstimatedMa());
28186         assertNull(calibrator.getEstimatedSx());
28187         assertNull(calibrator.getEstimatedSy());
28188         assertNull(calibrator.getEstimatedSz());
28189         assertNull(calibrator.getEstimatedMxy());
28190         assertNull(calibrator.getEstimatedMxz());
28191         assertNull(calibrator.getEstimatedMyx());
28192         assertNull(calibrator.getEstimatedMyz());
28193         assertNull(calibrator.getEstimatedMzx());
28194         assertNull(calibrator.getEstimatedMzy());
28195         assertNull(calibrator.getEstimatedCovariance());
28196         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28197         assertNotNull(calibrator.getGroundTruthGravityNorm());
28198         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28199         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28200         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28201                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28202         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28203         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28204         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28205 
28206         // Force IllegalArgumentException
28207         final Acceleration invalidGravityNorm = new Acceleration(
28208                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28209 
28210         calibrator = null;
28211         try {
28212             calibrator = new KnownGravityNormAccelerometerCalibrator(
28213                     invalidGravityNorm, bias);
28214             fail("IllegalArgumentException expected but not thrown");
28215         } catch (final IllegalArgumentException ignore) {
28216         }
28217         try {
28218             calibrator = new KnownGravityNormAccelerometerCalibrator(
28219                     gravityNorm, new double[1]);
28220             fail("IllegalArgumentException expected but not thrown");
28221         } catch (final IllegalArgumentException ignore) {
28222         }
28223         assertNull(calibrator);
28224     }
28225 
28226     @Test
28227     public void testConstructor212() throws WrongSizeException {
28228         final Matrix ba = generateBa();
28229         final double[] bias = ba.getBuffer();
28230         final double biasX = ba.getElementAtIndex(0);
28231         final double biasY = ba.getElementAtIndex(1);
28232         final double biasZ = ba.getElementAtIndex(2);
28233 
28234         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28235         final double latitude = Math.toRadians(
28236                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28237         final double longitude = Math.toRadians(
28238                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28239         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28240         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28241         final NEDVelocity nedVelocity = new NEDVelocity();
28242         final ECEFPosition ecefPosition = new ECEFPosition();
28243         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28244         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28245                 ecefPosition, ecefVelocity);
28246         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28247                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28248         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28249 
28250         KnownGravityNormAccelerometerCalibrator calibrator =
28251                 new KnownGravityNormAccelerometerCalibrator(
28252                         gravityNorm, bias, this);
28253 
28254         // check default values
28255         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28256         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28257         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28258         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28259         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28260         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28261         final Acceleration bx2 = new Acceleration(0.0,
28262                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28263         calibrator.getInitialBiasXAsAcceleration(bx2);
28264         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28265         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28266         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28267         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28268         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28269         final Acceleration by2 = new Acceleration(0.0,
28270                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28271         calibrator.getInitialBiasYAsAcceleration(by2);
28272         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28273         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28274         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28275         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28276         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28277         final Acceleration bz2 = new Acceleration(0.0,
28278                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28279         calibrator.getInitialBiasZAsAcceleration(bz2);
28280         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28281         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28282         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28283         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28284         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28285         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28286         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28287         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28288         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28289         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28290         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28291         final double[] bias1 = calibrator.getInitialBias();
28292         assertArrayEquals(bias1, bias, 0.0);
28293         final double[] bias2 = new double[3];
28294         calibrator.getInitialBias(bias2);
28295         assertArrayEquals(bias1, bias2, 0.0);
28296         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28297         assertEquals(b1, ba);
28298         final Matrix b2 = new Matrix(3, 1);
28299         calibrator.getInitialBiasAsMatrix(b2);
28300         assertEquals(b1, b2);
28301         final Matrix ma1 = calibrator.getInitialMa();
28302         assertEquals(ma1, new Matrix(3, 3));
28303         final Matrix ma2 = new Matrix(3, 3);
28304         calibrator.getInitialMa(ma2);
28305         assertEquals(ma1, ma2);
28306         assertNull(calibrator.getMeasurements());
28307         assertFalse(calibrator.isCommonAxisUsed());
28308         assertSame(calibrator.getListener(), this);
28309         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28310         assertFalse(calibrator.isReady());
28311         assertFalse(calibrator.isRunning());
28312         assertNull(calibrator.getEstimatedBiases());
28313         assertFalse(calibrator.getEstimatedBiases(null));
28314         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28315         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28316         assertNull(calibrator.getEstimatedBiasFx());
28317         assertNull(calibrator.getEstimatedBiasFy());
28318         assertNull(calibrator.getEstimatedBiasFz());
28319         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28320         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28321         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28322         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28323         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28324         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28325         assertNull(calibrator.getEstimatedMa());
28326         assertNull(calibrator.getEstimatedSx());
28327         assertNull(calibrator.getEstimatedSy());
28328         assertNull(calibrator.getEstimatedSz());
28329         assertNull(calibrator.getEstimatedMxy());
28330         assertNull(calibrator.getEstimatedMxz());
28331         assertNull(calibrator.getEstimatedMyx());
28332         assertNull(calibrator.getEstimatedMyz());
28333         assertNull(calibrator.getEstimatedMzx());
28334         assertNull(calibrator.getEstimatedMzy());
28335         assertNull(calibrator.getEstimatedCovariance());
28336         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28337         assertNotNull(calibrator.getGroundTruthGravityNorm());
28338         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28339         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28340         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28341                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28342         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28343         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28344         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28345 
28346         // Force IllegalArgumentException
28347         final Acceleration invalidGravityNorm = new Acceleration(
28348                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28349 
28350         calibrator = null;
28351         try {
28352             calibrator = new KnownGravityNormAccelerometerCalibrator(
28353                     invalidGravityNorm, bias, this);
28354             fail("IllegalArgumentException expected but not thrown");
28355         } catch (final IllegalArgumentException ignore) {
28356         }
28357         try {
28358             calibrator = new KnownGravityNormAccelerometerCalibrator(
28359                     gravityNorm, new double[1], this);
28360             fail("IllegalArgumentException expected but not thrown");
28361         } catch (final IllegalArgumentException ignore) {
28362         }
28363         assertNull(calibrator);
28364     }
28365 
28366     @Test
28367     public void testConstructor213() throws WrongSizeException {
28368         final Collection<StandardDeviationBodyKinematics> measurements =
28369                 Collections.emptyList();
28370 
28371         final Matrix ba = generateBa();
28372         final double[] bias = ba.getBuffer();
28373         final double biasX = ba.getElementAtIndex(0);
28374         final double biasY = ba.getElementAtIndex(1);
28375         final double biasZ = ba.getElementAtIndex(2);
28376 
28377         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28378         final double latitude = Math.toRadians(
28379                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28380         final double longitude = Math.toRadians(
28381                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28382         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28383         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28384         final NEDVelocity nedVelocity = new NEDVelocity();
28385         final ECEFPosition ecefPosition = new ECEFPosition();
28386         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28387         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28388                 ecefPosition, ecefVelocity);
28389         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28390                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28391         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28392 
28393         KnownGravityNormAccelerometerCalibrator calibrator =
28394                 new KnownGravityNormAccelerometerCalibrator(
28395                         gravityNorm, measurements, bias);
28396 
28397         // check default values
28398         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28399         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28400         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28401         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28402         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28403         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28404         final Acceleration bx2 = new Acceleration(0.0,
28405                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28406         calibrator.getInitialBiasXAsAcceleration(bx2);
28407         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28408         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28409         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28410         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28411         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28412         final Acceleration by2 = new Acceleration(0.0,
28413                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28414         calibrator.getInitialBiasYAsAcceleration(by2);
28415         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28416         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28417         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28418         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28419         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28420         final Acceleration bz2 = new Acceleration(0.0,
28421                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28422         calibrator.getInitialBiasZAsAcceleration(bz2);
28423         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28424         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28425         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28426         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28427         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28428         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28429         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28430         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28431         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28432         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28433         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28434         final double[] bias1 = calibrator.getInitialBias();
28435         assertArrayEquals(bias1, bias, 0.0);
28436         final double[] bias2 = new double[3];
28437         calibrator.getInitialBias(bias2);
28438         assertArrayEquals(bias1, bias2, 0.0);
28439         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28440         assertEquals(b1, ba);
28441         final Matrix b2 = new Matrix(3, 1);
28442         calibrator.getInitialBiasAsMatrix(b2);
28443         assertEquals(b1, b2);
28444         final Matrix ma1 = calibrator.getInitialMa();
28445         assertEquals(ma1, new Matrix(3, 3));
28446         final Matrix ma2 = new Matrix(3, 3);
28447         calibrator.getInitialMa(ma2);
28448         assertEquals(ma1, ma2);
28449         assertSame(calibrator.getMeasurements(), measurements);
28450         assertFalse(calibrator.isCommonAxisUsed());
28451         assertNull(calibrator.getListener());
28452         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28453         assertFalse(calibrator.isReady());
28454         assertFalse(calibrator.isRunning());
28455         assertNull(calibrator.getEstimatedBiases());
28456         assertFalse(calibrator.getEstimatedBiases(null));
28457         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28458         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28459         assertNull(calibrator.getEstimatedBiasFx());
28460         assertNull(calibrator.getEstimatedBiasFy());
28461         assertNull(calibrator.getEstimatedBiasFz());
28462         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28463         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28464         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28465         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28466         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28467         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28468         assertNull(calibrator.getEstimatedMa());
28469         assertNull(calibrator.getEstimatedSx());
28470         assertNull(calibrator.getEstimatedSy());
28471         assertNull(calibrator.getEstimatedSz());
28472         assertNull(calibrator.getEstimatedMxy());
28473         assertNull(calibrator.getEstimatedMxz());
28474         assertNull(calibrator.getEstimatedMyx());
28475         assertNull(calibrator.getEstimatedMyz());
28476         assertNull(calibrator.getEstimatedMzx());
28477         assertNull(calibrator.getEstimatedMzy());
28478         assertNull(calibrator.getEstimatedCovariance());
28479         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28480         assertNotNull(calibrator.getGroundTruthGravityNorm());
28481         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28482         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28483         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28484                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28485         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28486         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28487         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28488 
28489         // Force IllegalArgumentException
28490         final Acceleration invalidGravityNorm = new Acceleration(
28491                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28492 
28493         calibrator = null;
28494         try {
28495             calibrator = new KnownGravityNormAccelerometerCalibrator(
28496                     invalidGravityNorm, measurements, bias);
28497             fail("IllegalArgumentException expected but not thrown");
28498         } catch (final IllegalArgumentException ignore) {
28499         }
28500         try {
28501             calibrator = new KnownGravityNormAccelerometerCalibrator(
28502                     gravityNorm, measurements, new double[1]);
28503             fail("IllegalArgumentException expected but not thrown");
28504         } catch (final IllegalArgumentException ignore) {
28505         }
28506         assertNull(calibrator);
28507     }
28508 
28509     @Test
28510     public void testConstructor214() throws WrongSizeException {
28511         final Collection<StandardDeviationBodyKinematics> measurements =
28512                 Collections.emptyList();
28513 
28514         final Matrix ba = generateBa();
28515         final double[] bias = ba.getBuffer();
28516         final double biasX = ba.getElementAtIndex(0);
28517         final double biasY = ba.getElementAtIndex(1);
28518         final double biasZ = ba.getElementAtIndex(2);
28519 
28520         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28521         final double latitude = Math.toRadians(
28522                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28523         final double longitude = Math.toRadians(
28524                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28525         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28526         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28527         final NEDVelocity nedVelocity = new NEDVelocity();
28528         final ECEFPosition ecefPosition = new ECEFPosition();
28529         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28530         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28531                 ecefPosition, ecefVelocity);
28532         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28533                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28534         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28535 
28536         KnownGravityNormAccelerometerCalibrator calibrator =
28537                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
28538                         measurements, bias, this);
28539 
28540         // check default values
28541         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28542         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28543         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28544         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28545         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28546         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28547         final Acceleration bx2 = new Acceleration(0.0,
28548                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28549         calibrator.getInitialBiasXAsAcceleration(bx2);
28550         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28551         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28552         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28553         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28554         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28555         final Acceleration by2 = new Acceleration(0.0,
28556                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28557         calibrator.getInitialBiasYAsAcceleration(by2);
28558         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28559         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28560         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28561         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28562         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28563         final Acceleration bz2 = new Acceleration(0.0,
28564                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28565         calibrator.getInitialBiasZAsAcceleration(bz2);
28566         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28567         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28568         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28569         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28570         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28571         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28572         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28573         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28574         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28575         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28576         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28577         final double[] bias1 = calibrator.getInitialBias();
28578         assertArrayEquals(bias1, bias, 0.0);
28579         final double[] bias2 = new double[3];
28580         calibrator.getInitialBias(bias2);
28581         assertArrayEquals(bias1, bias2, 0.0);
28582         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28583         assertEquals(b1, ba);
28584         final Matrix b2 = new Matrix(3, 1);
28585         calibrator.getInitialBiasAsMatrix(b2);
28586         assertEquals(b1, b2);
28587         final Matrix ma1 = calibrator.getInitialMa();
28588         assertEquals(ma1, new Matrix(3, 3));
28589         final Matrix ma2 = new Matrix(3, 3);
28590         calibrator.getInitialMa(ma2);
28591         assertEquals(ma1, ma2);
28592         assertSame(calibrator.getMeasurements(), measurements);
28593         assertFalse(calibrator.isCommonAxisUsed());
28594         assertSame(calibrator.getListener(), this);
28595         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28596         assertFalse(calibrator.isReady());
28597         assertFalse(calibrator.isRunning());
28598         assertNull(calibrator.getEstimatedBiases());
28599         assertFalse(calibrator.getEstimatedBiases(null));
28600         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28601         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28602         assertNull(calibrator.getEstimatedBiasFx());
28603         assertNull(calibrator.getEstimatedBiasFy());
28604         assertNull(calibrator.getEstimatedBiasFz());
28605         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28606         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28607         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28608         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28609         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28610         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28611         assertNull(calibrator.getEstimatedMa());
28612         assertNull(calibrator.getEstimatedSx());
28613         assertNull(calibrator.getEstimatedSy());
28614         assertNull(calibrator.getEstimatedSz());
28615         assertNull(calibrator.getEstimatedMxy());
28616         assertNull(calibrator.getEstimatedMxz());
28617         assertNull(calibrator.getEstimatedMyx());
28618         assertNull(calibrator.getEstimatedMyz());
28619         assertNull(calibrator.getEstimatedMzx());
28620         assertNull(calibrator.getEstimatedMzy());
28621         assertNull(calibrator.getEstimatedCovariance());
28622         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28623         assertNotNull(calibrator.getGroundTruthGravityNorm());
28624         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28625         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28626         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28627                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28628         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28629         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28630         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28631 
28632         // Force IllegalArgumentException
28633         final Acceleration invalidGravityNorm = new Acceleration(
28634                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28635 
28636         calibrator = null;
28637         try {
28638             calibrator = new KnownGravityNormAccelerometerCalibrator(
28639                     invalidGravityNorm, measurements, bias, this);
28640             fail("IllegalArgumentException expected but not thrown");
28641         } catch (final IllegalArgumentException ignore) {
28642         }
28643         try {
28644             calibrator = new KnownGravityNormAccelerometerCalibrator(
28645                     gravityNorm, measurements,
28646                     new double[1], this);
28647             fail("IllegalArgumentException expected but not thrown");
28648         } catch (final IllegalArgumentException ignore) {
28649         }
28650         assertNull(calibrator);
28651     }
28652 
28653     @Test
28654     public void testConstructor215() throws WrongSizeException {
28655         final Matrix ba = generateBa();
28656         final double[] bias = ba.getBuffer();
28657         final double biasX = ba.getElementAtIndex(0);
28658         final double biasY = ba.getElementAtIndex(1);
28659         final double biasZ = ba.getElementAtIndex(2);
28660 
28661         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28662         final double latitude = Math.toRadians(
28663                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28664         final double longitude = Math.toRadians(
28665                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28666         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28667         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28668         final NEDVelocity nedVelocity = new NEDVelocity();
28669         final ECEFPosition ecefPosition = new ECEFPosition();
28670         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28671         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28672                 ecefPosition, ecefVelocity);
28673         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28674                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28675         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28676 
28677         KnownGravityNormAccelerometerCalibrator calibrator =
28678                 new KnownGravityNormAccelerometerCalibrator(
28679                         gravityNorm, true, bias);
28680 
28681         // check default values
28682         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28683         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28684         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28685         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28686         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28687         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28688         final Acceleration bx2 = new Acceleration(0.0,
28689                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28690         calibrator.getInitialBiasXAsAcceleration(bx2);
28691         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28692         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28693         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28694         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28695         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28696         final Acceleration by2 = new Acceleration(0.0,
28697                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28698         calibrator.getInitialBiasYAsAcceleration(by2);
28699         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28700         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28701         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28702         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28703         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28704         final Acceleration bz2 = new Acceleration(0.0,
28705                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28706         calibrator.getInitialBiasZAsAcceleration(bz2);
28707         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28708         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28709         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28710         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28711         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28712         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28713         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28714         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28715         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28716         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28717         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28718         final double[] bias1 = calibrator.getInitialBias();
28719         assertArrayEquals(bias1, bias, 0.0);
28720         final double[] bias2 = new double[3];
28721         calibrator.getInitialBias(bias2);
28722         assertArrayEquals(bias1, bias2, 0.0);
28723         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28724         assertEquals(b1, ba);
28725         final Matrix b2 = new Matrix(3, 1);
28726         calibrator.getInitialBiasAsMatrix(b2);
28727         assertEquals(b1, b2);
28728         final Matrix ma1 = calibrator.getInitialMa();
28729         assertEquals(ma1, new Matrix(3, 3));
28730         final Matrix ma2 = new Matrix(3, 3);
28731         calibrator.getInitialMa(ma2);
28732         assertEquals(ma1, ma2);
28733         assertNull(calibrator.getMeasurements());
28734         assertTrue(calibrator.isCommonAxisUsed());
28735         assertNull(calibrator.getListener());
28736         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28737         assertFalse(calibrator.isReady());
28738         assertFalse(calibrator.isRunning());
28739         assertNull(calibrator.getEstimatedBiases());
28740         assertFalse(calibrator.getEstimatedBiases(null));
28741         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28742         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28743         assertNull(calibrator.getEstimatedBiasFx());
28744         assertNull(calibrator.getEstimatedBiasFy());
28745         assertNull(calibrator.getEstimatedBiasFz());
28746         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28747         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28748         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28749         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28750         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28751         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28752         assertNull(calibrator.getEstimatedMa());
28753         assertNull(calibrator.getEstimatedSx());
28754         assertNull(calibrator.getEstimatedSy());
28755         assertNull(calibrator.getEstimatedSz());
28756         assertNull(calibrator.getEstimatedMxy());
28757         assertNull(calibrator.getEstimatedMxz());
28758         assertNull(calibrator.getEstimatedMyx());
28759         assertNull(calibrator.getEstimatedMyz());
28760         assertNull(calibrator.getEstimatedMzx());
28761         assertNull(calibrator.getEstimatedMzy());
28762         assertNull(calibrator.getEstimatedCovariance());
28763         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28764         assertNotNull(calibrator.getGroundTruthGravityNorm());
28765         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28766         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28767         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28768                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28769         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28770         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28771         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28772 
28773         // Force IllegalArgumentException
28774         final Acceleration invalidGravityNorm = new Acceleration(
28775                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28776 
28777         calibrator = null;
28778         try {
28779             calibrator = new KnownGravityNormAccelerometerCalibrator(
28780                     invalidGravityNorm, true, bias);
28781             fail("IllegalArgumentException expected but not thrown");
28782         } catch (final IllegalArgumentException ignore) {
28783         }
28784         try {
28785             calibrator = new KnownGravityNormAccelerometerCalibrator(
28786                     gravityNorm, true, new double[1]);
28787             fail("IllegalArgumentException expected but not thrown");
28788         } catch (final IllegalArgumentException ignore) {
28789         }
28790         assertNull(calibrator);
28791     }
28792 
28793     @Test
28794     public void testConstructor216() throws WrongSizeException {
28795         final Matrix ba = generateBa();
28796         final double[] bias = ba.getBuffer();
28797         final double biasX = ba.getElementAtIndex(0);
28798         final double biasY = ba.getElementAtIndex(1);
28799         final double biasZ = ba.getElementAtIndex(2);
28800 
28801         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28802         final double latitude = Math.toRadians(
28803                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28804         final double longitude = Math.toRadians(
28805                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28806         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28807         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28808         final NEDVelocity nedVelocity = new NEDVelocity();
28809         final ECEFPosition ecefPosition = new ECEFPosition();
28810         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28811         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28812                 ecefPosition, ecefVelocity);
28813         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28814                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28815         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28816 
28817         KnownGravityNormAccelerometerCalibrator calibrator =
28818                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
28819                         true, bias, this);
28820 
28821         // check default values
28822         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28823         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28824         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28825         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28826         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28827         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28828         final Acceleration bx2 = new Acceleration(0.0,
28829                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28830         calibrator.getInitialBiasXAsAcceleration(bx2);
28831         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28832         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28833         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28834         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28835         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28836         final Acceleration by2 = new Acceleration(0.0,
28837                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28838         calibrator.getInitialBiasYAsAcceleration(by2);
28839         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28840         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28841         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28842         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28843         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28844         final Acceleration bz2 = new Acceleration(0.0,
28845                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28846         calibrator.getInitialBiasZAsAcceleration(bz2);
28847         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28848         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28849         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28850         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28851         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28852         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28853         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28854         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28855         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28856         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28857         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28858         final double[] bias1 = calibrator.getInitialBias();
28859         assertArrayEquals(bias1, bias, 0.0);
28860         final double[] bias2 = new double[3];
28861         calibrator.getInitialBias(bias2);
28862         assertArrayEquals(bias1, bias2, 0.0);
28863         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28864         assertEquals(b1, ba);
28865         final Matrix b2 = new Matrix(3, 1);
28866         calibrator.getInitialBiasAsMatrix(b2);
28867         assertEquals(b1, b2);
28868         final Matrix ma1 = calibrator.getInitialMa();
28869         assertEquals(ma1, new Matrix(3, 3));
28870         final Matrix ma2 = new Matrix(3, 3);
28871         calibrator.getInitialMa(ma2);
28872         assertEquals(ma1, ma2);
28873         assertNull(calibrator.getMeasurements());
28874         assertTrue(calibrator.isCommonAxisUsed());
28875         assertSame(calibrator.getListener(), this);
28876         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28877         assertFalse(calibrator.isReady());
28878         assertFalse(calibrator.isRunning());
28879         assertNull(calibrator.getEstimatedBiases());
28880         assertFalse(calibrator.getEstimatedBiases(null));
28881         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28882         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28883         assertNull(calibrator.getEstimatedBiasFx());
28884         assertNull(calibrator.getEstimatedBiasFy());
28885         assertNull(calibrator.getEstimatedBiasFz());
28886         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28887         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28888         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28889         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28890         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28891         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28892         assertNull(calibrator.getEstimatedMa());
28893         assertNull(calibrator.getEstimatedSx());
28894         assertNull(calibrator.getEstimatedSy());
28895         assertNull(calibrator.getEstimatedSz());
28896         assertNull(calibrator.getEstimatedMxy());
28897         assertNull(calibrator.getEstimatedMxz());
28898         assertNull(calibrator.getEstimatedMyx());
28899         assertNull(calibrator.getEstimatedMyz());
28900         assertNull(calibrator.getEstimatedMzx());
28901         assertNull(calibrator.getEstimatedMzy());
28902         assertNull(calibrator.getEstimatedCovariance());
28903         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28904         assertNotNull(calibrator.getGroundTruthGravityNorm());
28905         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28906         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28907         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28908                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28909         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28910         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28911         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28912 
28913         // Force IllegalArgumentException
28914         final Acceleration invalidGravityNorm = new Acceleration(
28915                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28916 
28917         calibrator = null;
28918         try {
28919             calibrator = new KnownGravityNormAccelerometerCalibrator(
28920                     invalidGravityNorm, true,
28921                     bias, this);
28922             fail("IllegalArgumentException expected but not thrown");
28923         } catch (final IllegalArgumentException ignore) {
28924         }
28925         try {
28926             calibrator = new KnownGravityNormAccelerometerCalibrator(
28927                     gravityNorm, true,
28928                     new double[1], this);
28929             fail("IllegalArgumentException expected but not thrown");
28930         } catch (final IllegalArgumentException ignore) {
28931         }
28932         assertNull(calibrator);
28933     }
28934 
28935     @Test
28936     public void testConstructor217() throws WrongSizeException {
28937         final Collection<StandardDeviationBodyKinematics> measurements =
28938                 Collections.emptyList();
28939 
28940         final Matrix ba = generateBa();
28941         final double[] bias = ba.getBuffer();
28942         final double biasX = ba.getElementAtIndex(0);
28943         final double biasY = ba.getElementAtIndex(1);
28944         final double biasZ = ba.getElementAtIndex(2);
28945 
28946         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28947         final double latitude = Math.toRadians(
28948                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28949         final double longitude = Math.toRadians(
28950                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28951         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28952         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28953         final NEDVelocity nedVelocity = new NEDVelocity();
28954         final ECEFPosition ecefPosition = new ECEFPosition();
28955         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28956         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28957                 ecefPosition, ecefVelocity);
28958         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28959                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28960         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
28961 
28962         KnownGravityNormAccelerometerCalibrator calibrator =
28963                 new KnownGravityNormAccelerometerCalibrator(
28964                         gravityNorm, measurements,
28965                         true, bias);
28966 
28967         // check default values
28968         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28969         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28970         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28971         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28972         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28973         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28974         final Acceleration bx2 = new Acceleration(0.0,
28975                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28976         calibrator.getInitialBiasXAsAcceleration(bx2);
28977         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28978         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28979         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28980         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28981         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28982         final Acceleration by2 = new Acceleration(0.0,
28983                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28984         calibrator.getInitialBiasYAsAcceleration(by2);
28985         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28986         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28987         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28988         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28989         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28990         final Acceleration bz2 = new Acceleration(0.0,
28991                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28992         calibrator.getInitialBiasZAsAcceleration(bz2);
28993         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28994         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28995         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28996         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28997         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28998         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28999         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29000         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29001         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29002         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29003         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29004         final double[] bias1 = calibrator.getInitialBias();
29005         assertArrayEquals(bias1, bias, 0.0);
29006         final double[] bias2 = new double[3];
29007         calibrator.getInitialBias(bias2);
29008         assertArrayEquals(bias1, bias2, 0.0);
29009         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29010         assertEquals(b1, ba);
29011         final Matrix b2 = new Matrix(3, 1);
29012         calibrator.getInitialBiasAsMatrix(b2);
29013         assertEquals(b1, b2);
29014         final Matrix ma1 = calibrator.getInitialMa();
29015         assertEquals(ma1, new Matrix(3, 3));
29016         final Matrix ma2 = new Matrix(3, 3);
29017         calibrator.getInitialMa(ma2);
29018         assertEquals(ma1, ma2);
29019         assertSame(calibrator.getMeasurements(), measurements);
29020         assertTrue(calibrator.isCommonAxisUsed());
29021         assertNull(calibrator.getListener());
29022         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
29023         assertFalse(calibrator.isReady());
29024         assertFalse(calibrator.isRunning());
29025         assertNull(calibrator.getEstimatedBiases());
29026         assertFalse(calibrator.getEstimatedBiases(null));
29027         assertNull(calibrator.getEstimatedBiasesAsMatrix());
29028         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29029         assertNull(calibrator.getEstimatedBiasFx());
29030         assertNull(calibrator.getEstimatedBiasFy());
29031         assertNull(calibrator.getEstimatedBiasFz());
29032         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29033         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29034         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29035         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29036         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29037         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29038         assertNull(calibrator.getEstimatedMa());
29039         assertNull(calibrator.getEstimatedSx());
29040         assertNull(calibrator.getEstimatedSy());
29041         assertNull(calibrator.getEstimatedSz());
29042         assertNull(calibrator.getEstimatedMxy());
29043         assertNull(calibrator.getEstimatedMxz());
29044         assertNull(calibrator.getEstimatedMyx());
29045         assertNull(calibrator.getEstimatedMyz());
29046         assertNull(calibrator.getEstimatedMzx());
29047         assertNull(calibrator.getEstimatedMzy());
29048         assertNull(calibrator.getEstimatedCovariance());
29049         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29050         assertNotNull(calibrator.getGroundTruthGravityNorm());
29051         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29052         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29053         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29054                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29055         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29056         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29057         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29058 
29059         // Force IllegalArgumentException
29060         final Acceleration invalidGravityNorm = new Acceleration(
29061                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29062 
29063         calibrator = null;
29064         try {
29065             calibrator = new KnownGravityNormAccelerometerCalibrator(
29066                     invalidGravityNorm, measurements,
29067                     true, bias);
29068             fail("IllegalArgumentException expected but not thrown");
29069         } catch (final IllegalArgumentException ignore) {
29070         }
29071         try {
29072             calibrator = new KnownGravityNormAccelerometerCalibrator(
29073                     gravityNorm, measurements,
29074                     true, new double[1]);
29075             fail("IllegalArgumentException expected but not thrown");
29076         } catch (final IllegalArgumentException ignore) {
29077         }
29078         assertNull(calibrator);
29079     }
29080 
29081     @Test
29082     public void testConstructor218() throws WrongSizeException {
29083         final Collection<StandardDeviationBodyKinematics> measurements =
29084                 Collections.emptyList();
29085 
29086         final Matrix ba = generateBa();
29087         final double[] bias = ba.getBuffer();
29088         final double biasX = ba.getElementAtIndex(0);
29089         final double biasY = ba.getElementAtIndex(1);
29090         final double biasZ = ba.getElementAtIndex(2);
29091 
29092         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29093         final double latitude = Math.toRadians(
29094                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29095         final double longitude = Math.toRadians(
29096                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29097         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29098         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29099         final NEDVelocity nedVelocity = new NEDVelocity();
29100         final ECEFPosition ecefPosition = new ECEFPosition();
29101         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29102         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29103                 ecefPosition, ecefVelocity);
29104         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29105                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29106         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
29107 
29108         KnownGravityNormAccelerometerCalibrator calibrator =
29109                 new KnownGravityNormAccelerometerCalibrator(
29110                         gravityNorm, measurements,
29111                         true, bias, this);
29112 
29113         // check default values
29114         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29115         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29116         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29117         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29118         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29119         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29120         final Acceleration bx2 = new Acceleration(0.0,
29121                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29122         calibrator.getInitialBiasXAsAcceleration(bx2);
29123         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29124         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29125         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29126         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29127         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29128         final Acceleration by2 = new Acceleration(0.0,
29129                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29130         calibrator.getInitialBiasYAsAcceleration(by2);
29131         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29132         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29133         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29134         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29135         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29136         final Acceleration bz2 = new Acceleration(0.0,
29137                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29138         calibrator.getInitialBiasZAsAcceleration(bz2);
29139         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29140         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29141         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29142         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29143         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29144         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29145         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29146         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29147         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29148         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29149         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29150         final double[] bias1 = calibrator.getInitialBias();
29151         assertArrayEquals(bias1, bias, 0.0);
29152         final double[] bias2 = new double[3];
29153         calibrator.getInitialBias(bias2);
29154         assertArrayEquals(bias1, bias2, 0.0);
29155         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29156         assertEquals(b1, ba);
29157         final Matrix b2 = new Matrix(3, 1);
29158         calibrator.getInitialBiasAsMatrix(b2);
29159         assertEquals(b1, b2);
29160         final Matrix ma1 = calibrator.getInitialMa();
29161         assertEquals(ma1, new Matrix(3, 3));
29162         final Matrix ma2 = new Matrix(3, 3);
29163         calibrator.getInitialMa(ma2);
29164         assertEquals(ma1, ma2);
29165         assertSame(calibrator.getMeasurements(), measurements);
29166         assertTrue(calibrator.isCommonAxisUsed());
29167         assertSame(calibrator.getListener(), this);
29168         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
29169         assertFalse(calibrator.isReady());
29170         assertFalse(calibrator.isRunning());
29171         assertNull(calibrator.getEstimatedBiases());
29172         assertFalse(calibrator.getEstimatedBiases(null));
29173         assertNull(calibrator.getEstimatedBiasesAsMatrix());
29174         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29175         assertNull(calibrator.getEstimatedBiasFx());
29176         assertNull(calibrator.getEstimatedBiasFy());
29177         assertNull(calibrator.getEstimatedBiasFz());
29178         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29179         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29180         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29181         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29182         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29183         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29184         assertNull(calibrator.getEstimatedMa());
29185         assertNull(calibrator.getEstimatedSx());
29186         assertNull(calibrator.getEstimatedSy());
29187         assertNull(calibrator.getEstimatedSz());
29188         assertNull(calibrator.getEstimatedMxy());
29189         assertNull(calibrator.getEstimatedMxz());
29190         assertNull(calibrator.getEstimatedMyx());
29191         assertNull(calibrator.getEstimatedMyz());
29192         assertNull(calibrator.getEstimatedMzx());
29193         assertNull(calibrator.getEstimatedMzy());
29194         assertNull(calibrator.getEstimatedCovariance());
29195         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29196         assertNotNull(calibrator.getGroundTruthGravityNorm());
29197         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29198         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29199         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29200                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29201         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29202         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29203         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29204 
29205         // Force IllegalArgumentException
29206         final Acceleration invalidGravityNorm = new Acceleration(
29207                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29208 
29209         calibrator = null;
29210         try {
29211             calibrator = new KnownGravityNormAccelerometerCalibrator(
29212                     invalidGravityNorm, measurements,
29213                     true, bias, this);
29214             fail("IllegalArgumentException expected but not thrown");
29215         } catch (final IllegalArgumentException ignore) {
29216         }
29217         try {
29218             calibrator = new KnownGravityNormAccelerometerCalibrator(
29219                     gravityNorm, measurements,
29220                     true, new double[1], this);
29221             fail("IllegalArgumentException expected but not thrown");
29222         } catch (final IllegalArgumentException ignore) {
29223         }
29224         assertNull(calibrator);
29225     }
29226 
29227     @Test
29228     public void testConstructor219() throws WrongSizeException {
29229         final Matrix ba = generateBa();
29230         final double[] bias = ba.getBuffer();
29231         final double biasX = ba.getElementAtIndex(0);
29232         final double biasY = ba.getElementAtIndex(1);
29233         final double biasZ = ba.getElementAtIndex(2);
29234 
29235         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29236         final double latitude = Math.toRadians(
29237                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29238         final double longitude = Math.toRadians(
29239                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29240         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29241         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29242         final NEDVelocity nedVelocity = new NEDVelocity();
29243         final ECEFPosition ecefPosition = new ECEFPosition();
29244         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29245         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29246                 ecefPosition, ecefVelocity);
29247         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29248                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29249         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
29250 
29251         KnownGravityNormAccelerometerCalibrator calibrator =
29252                 new KnownGravityNormAccelerometerCalibrator(
29253                         gravityNorm, ba);
29254 
29255         // check default values
29256         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29257         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29258         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29259         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29260         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29261         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29262         final Acceleration bx2 = new Acceleration(0.0,
29263                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29264         calibrator.getInitialBiasXAsAcceleration(bx2);
29265         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29266         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29267         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29268         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29269         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29270         final Acceleration by2 = new Acceleration(0.0,
29271                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29272         calibrator.getInitialBiasYAsAcceleration(by2);
29273         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29274         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29275         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29276         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29277         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29278         final Acceleration bz2 = new Acceleration(0.0,
29279                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29280         calibrator.getInitialBiasZAsAcceleration(bz2);
29281         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29282         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29283         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29284         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29285         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29286         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29287         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29288         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29289         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29290         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29291         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29292         final double[] bias1 = calibrator.getInitialBias();
29293         assertArrayEquals(bias1, bias, 0.0);
29294         final double[] bias2 = new double[3];
29295         calibrator.getInitialBias(bias2);
29296         assertArrayEquals(bias1, bias2, 0.0);
29297         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29298         assertEquals(b1, ba);
29299         final Matrix b2 = new Matrix(3, 1);
29300         calibrator.getInitialBiasAsMatrix(b2);
29301         assertEquals(b1, b2);
29302         final Matrix ma1 = calibrator.getInitialMa();
29303         assertEquals(ma1, new Matrix(3, 3));
29304         final Matrix ma2 = new Matrix(3, 3);
29305         calibrator.getInitialMa(ma2);
29306         assertEquals(ma1, ma2);
29307         assertNull(calibrator.getMeasurements());
29308         assertFalse(calibrator.isCommonAxisUsed());
29309         assertNull(calibrator.getListener());
29310         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
29311         assertFalse(calibrator.isReady());
29312         assertFalse(calibrator.isRunning());
29313         assertNull(calibrator.getEstimatedBiases());
29314         assertFalse(calibrator.getEstimatedBiases(null));
29315         assertNull(calibrator.getEstimatedBiasesAsMatrix());
29316         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29317         assertNull(calibrator.getEstimatedBiasFx());
29318         assertNull(calibrator.getEstimatedBiasFy());
29319         assertNull(calibrator.getEstimatedBiasFz());
29320         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29321         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29322         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29323         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29324         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29325         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29326         assertNull(calibrator.getEstimatedMa());
29327         assertNull(calibrator.getEstimatedSx());
29328         assertNull(calibrator.getEstimatedSy());
29329         assertNull(calibrator.getEstimatedSz());
29330         assertNull(calibrator.getEstimatedMxy());
29331         assertNull(calibrator.getEstimatedMxz());
29332         assertNull(calibrator.getEstimatedMyx());
29333         assertNull(calibrator.getEstimatedMyz());
29334         assertNull(calibrator.getEstimatedMzx());
29335         assertNull(calibrator.getEstimatedMzy());
29336         assertNull(calibrator.getEstimatedCovariance());
29337         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29338         assertNotNull(calibrator.getGroundTruthGravityNorm());
29339         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29340         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29341         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29342                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29343         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29344         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29345         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29346 
29347         // Force IllegalArgumentException
29348         final Acceleration invalidGravityNorm = new Acceleration(
29349                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29350 
29351         calibrator = null;
29352         try {
29353             calibrator = new KnownGravityNormAccelerometerCalibrator(
29354                     invalidGravityNorm, ba);
29355             fail("IllegalArgumentException expected but not thrown");
29356         } catch (final IllegalArgumentException ignore) {
29357         }
29358         try {
29359             calibrator = new KnownGravityNormAccelerometerCalibrator(
29360                     gravityNorm,
29361                     new Matrix(1, 1));
29362             fail("IllegalArgumentException expected but not thrown");
29363         } catch (final IllegalArgumentException ignore) {
29364         }
29365         try {
29366             calibrator = new KnownGravityNormAccelerometerCalibrator(
29367                     gravityNorm, new Matrix(1, 3));
29368             fail("IllegalArgumentException expected but not thrown");
29369         } catch (final IllegalArgumentException ignore) {
29370         }
29371         assertNull(calibrator);
29372     }
29373 
29374     @Test
29375     public void testConstructor220() throws WrongSizeException {
29376         final Matrix ba = generateBa();
29377         final double[] bias = ba.getBuffer();
29378         final double biasX = ba.getElementAtIndex(0);
29379         final double biasY = ba.getElementAtIndex(1);
29380         final double biasZ = ba.getElementAtIndex(2);
29381 
29382         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29383         final double latitude = Math.toRadians(
29384                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29385         final double longitude = Math.toRadians(
29386                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29387         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29388         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29389         final NEDVelocity nedVelocity = new NEDVelocity();
29390         final ECEFPosition ecefPosition = new ECEFPosition();
29391         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29392         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29393                 ecefPosition, ecefVelocity);
29394         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29395                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29396         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
29397 
29398         KnownGravityNormAccelerometerCalibrator calibrator =
29399                 new KnownGravityNormAccelerometerCalibrator(
29400                         gravityNorm, ba, this);
29401 
29402         // check default values
29403         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29404         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29405         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29406         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29407         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29408         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29409         final Acceleration bx2 = new Acceleration(0.0,
29410                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29411         calibrator.getInitialBiasXAsAcceleration(bx2);
29412         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29413         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29414         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29415         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29416         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29417         final Acceleration by2 = new Acceleration(0.0,
29418                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29419         calibrator.getInitialBiasYAsAcceleration(by2);
29420         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29421         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29422         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29423         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29424         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29425         final Acceleration bz2 = new Acceleration(0.0,
29426                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29427         calibrator.getInitialBiasZAsAcceleration(bz2);
29428         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29429         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29430         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29431         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29432         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29433         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29434         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29435         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29436         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29437         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29438         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29439         final double[] bias1 = calibrator.getInitialBias();
29440         assertArrayEquals(bias1, bias, 0.0);
29441         final double[] bias2 = new double[3];
29442         calibrator.getInitialBias(bias2);
29443         assertArrayEquals(bias1, bias2, 0.0);
29444         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29445         assertEquals(b1, ba);
29446         final Matrix b2 = new Matrix(3, 1);
29447         calibrator.getInitialBiasAsMatrix(b2);
29448         assertEquals(b1, b2);
29449         final Matrix ma1 = calibrator.getInitialMa();
29450         assertEquals(ma1, new Matrix(3, 3));
29451         final Matrix ma2 = new Matrix(3, 3);
29452         calibrator.getInitialMa(ma2);
29453         assertEquals(ma1, ma2);
29454         assertNull(calibrator.getMeasurements());
29455         assertFalse(calibrator.isCommonAxisUsed());
29456         assertSame(calibrator.getListener(), this);
29457         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
29458         assertFalse(calibrator.isReady());
29459         assertFalse(calibrator.isRunning());
29460         assertNull(calibrator.getEstimatedBiases());
29461         assertFalse(calibrator.getEstimatedBiases(null));
29462         assertNull(calibrator.getEstimatedBiasesAsMatrix());
29463         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29464         assertNull(calibrator.getEstimatedBiasFx());
29465         assertNull(calibrator.getEstimatedBiasFy());
29466         assertNull(calibrator.getEstimatedBiasFz());
29467         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29468         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29469         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29470         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29471         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29472         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29473         assertNull(calibrator.getEstimatedMa());
29474         assertNull(calibrator.getEstimatedSx());
29475         assertNull(calibrator.getEstimatedSy());
29476         assertNull(calibrator.getEstimatedSz());
29477         assertNull(calibrator.getEstimatedMxy());
29478         assertNull(calibrator.getEstimatedMxz());
29479         assertNull(calibrator.getEstimatedMyx());
29480         assertNull(calibrator.getEstimatedMyz());
29481         assertNull(calibrator.getEstimatedMzx());
29482         assertNull(calibrator.getEstimatedMzy());
29483         assertNull(calibrator.getEstimatedCovariance());
29484         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29485         assertNotNull(calibrator.getGroundTruthGravityNorm());
29486         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29487         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29488         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29489                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29490         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29491         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29492         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29493 
29494         // Force IllegalArgumentException
29495         final Acceleration invalidGravityNorm = new Acceleration(
29496                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29497 
29498         calibrator = null;
29499         try {
29500             calibrator = new KnownGravityNormAccelerometerCalibrator(
29501                     invalidGravityNorm, ba);
29502             fail("IllegalArgumentException expected but not thrown");
29503         } catch (final IllegalArgumentException ignore) {
29504         }
29505         try {
29506             calibrator = new KnownGravityNormAccelerometerCalibrator(
29507                     gravityNorm, new Matrix(1, 1),
29508                     this);
29509             fail("IllegalArgumentException expected but not thrown");
29510         } catch (final IllegalArgumentException ignore) {
29511         }
29512         try {
29513             calibrator = new KnownGravityNormAccelerometerCalibrator(
29514                     gravityNorm, new Matrix(1, 3),
29515                     this);
29516             fail("IllegalArgumentException expected but not thrown");
29517         } catch (final IllegalArgumentException ignore) {
29518         }
29519         assertNull(calibrator);
29520     }
29521 
29522     @Test
29523     public void testConstructor221() throws WrongSizeException {
29524         final Collection<StandardDeviationBodyKinematics> measurements =
29525                 Collections.emptyList();
29526 
29527         final Matrix ba = generateBa();
29528         final double[] bias = ba.getBuffer();
29529         final double biasX = ba.getElementAtIndex(0);
29530         final double biasY = ba.getElementAtIndex(1);
29531         final double biasZ = ba.getElementAtIndex(2);
29532 
29533         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29534         final double latitude = Math.toRadians(
29535                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29536         final double longitude = Math.toRadians(
29537                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29538         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29539         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29540         final NEDVelocity nedVelocity = new NEDVelocity();
29541         final ECEFPosition ecefPosition = new ECEFPosition();
29542         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29543         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29544                 ecefPosition, ecefVelocity);
29545         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29546                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29547         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
29548 
29549         KnownGravityNormAccelerometerCalibrator calibrator =
29550                 new KnownGravityNormAccelerometerCalibrator(
29551                         gravityNorm, measurements, ba);
29552 
29553         // check default values
29554         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29555         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29556         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29557         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29558         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29559         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29560         final Acceleration bx2 = new Acceleration(0.0,
29561                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29562         calibrator.getInitialBiasXAsAcceleration(bx2);
29563         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29564         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29565         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29566         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29567         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29568         final Acceleration by2 = new Acceleration(0.0,
29569                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29570         calibrator.getInitialBiasYAsAcceleration(by2);
29571         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29572         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29573         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29574         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29575         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29576         final Acceleration bz2 = new Acceleration(0.0,
29577                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29578         calibrator.getInitialBiasZAsAcceleration(bz2);
29579         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29580         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29581         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29582         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29583         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29584         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29585         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29586         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29587         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29588         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29589         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29590         final double[] bias1 = calibrator.getInitialBias();
29591         assertArrayEquals(bias1, bias, 0.0);
29592         final double[] bias2 = new double[3];
29593         calibrator.getInitialBias(bias2);
29594         assertArrayEquals(bias1, bias2, 0.0);
29595         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29596         assertEquals(b1, ba);
29597         final Matrix b2 = new Matrix(3, 1);
29598         calibrator.getInitialBiasAsMatrix(b2);
29599         assertEquals(b1, b2);
29600         final Matrix ma1 = calibrator.getInitialMa();
29601         assertEquals(ma1, new Matrix(3, 3));
29602         final Matrix ma2 = new Matrix(3, 3);
29603         calibrator.getInitialMa(ma2);
29604         assertEquals(ma1, ma2);
29605         assertSame(calibrator.getMeasurements(), measurements);
29606         assertFalse(calibrator.isCommonAxisUsed());
29607         assertNull(calibrator.getListener());
29608         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
29609         assertFalse(calibrator.isReady());
29610         assertFalse(calibrator.isRunning());
29611         assertNull(calibrator.getEstimatedBiases());
29612         assertFalse(calibrator.getEstimatedBiases(null));
29613         assertNull(calibrator.getEstimatedBiasesAsMatrix());
29614         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29615         assertNull(calibrator.getEstimatedBiasFx());
29616         assertNull(calibrator.getEstimatedBiasFy());
29617         assertNull(calibrator.getEstimatedBiasFz());
29618         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29619         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29620         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29621         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29622         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29623         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29624         assertNull(calibrator.getEstimatedMa());
29625         assertNull(calibrator.getEstimatedSx());
29626         assertNull(calibrator.getEstimatedSy());
29627         assertNull(calibrator.getEstimatedSz());
29628         assertNull(calibrator.getEstimatedMxy());
29629         assertNull(calibrator.getEstimatedMxz());
29630         assertNull(calibrator.getEstimatedMyx());
29631         assertNull(calibrator.getEstimatedMyz());
29632         assertNull(calibrator.getEstimatedMzx());
29633         assertNull(calibrator.getEstimatedMzy());
29634         assertNull(calibrator.getEstimatedCovariance());
29635         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29636         assertNotNull(calibrator.getGroundTruthGravityNorm());
29637         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29638         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29639         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29640                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29641         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29642         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29643         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29644 
29645         // Force IllegalArgumentException
29646         final Acceleration invalidGravityNorm = new Acceleration(
29647                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29648 
29649         calibrator = null;
29650         try {
29651             calibrator = new KnownGravityNormAccelerometerCalibrator(
29652                     invalidGravityNorm, measurements, ba);
29653             fail("IllegalArgumentException expected but not thrown");
29654         } catch (final IllegalArgumentException ignore) {
29655         }
29656         try {
29657             calibrator = new KnownGravityNormAccelerometerCalibrator(
29658                     gravityNorm, measurements,
29659                     new Matrix(1, 1));
29660             fail("IllegalArgumentException expected but not thrown");
29661         } catch (final IllegalArgumentException ignore) {
29662         }
29663         try {
29664             calibrator = new KnownGravityNormAccelerometerCalibrator(
29665                     gravityNorm, measurements,
29666                     new Matrix(1, 3));
29667             fail("IllegalArgumentException expected but not thrown");
29668         } catch (final IllegalArgumentException ignore) {
29669         }
29670         assertNull(calibrator);
29671     }
29672 
29673     @Test
29674     public void testConstructor222() throws WrongSizeException {
29675         final Collection<StandardDeviationBodyKinematics> measurements =
29676                 Collections.emptyList();
29677 
29678         final Matrix ba = generateBa();
29679         final double[] bias = ba.getBuffer();
29680         final double biasX = ba.getElementAtIndex(0);
29681         final double biasY = ba.getElementAtIndex(1);
29682         final double biasZ = ba.getElementAtIndex(2);
29683 
29684         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29685         final double latitude = Math.toRadians(
29686                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29687         final double longitude = Math.toRadians(
29688                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29689         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29690         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29691         final NEDVelocity nedVelocity = new NEDVelocity();
29692         final ECEFPosition ecefPosition = new ECEFPosition();
29693         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29694         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29695                 ecefPosition, ecefVelocity);
29696         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29697                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29698         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
29699 
29700         KnownGravityNormAccelerometerCalibrator calibrator =
29701                 new KnownGravityNormAccelerometerCalibrator(
29702                         gravityNorm, measurements,
29703                         ba, this);
29704 
29705         // check default values
29706         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29707         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29708         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29709         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29710         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29711         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29712         final Acceleration bx2 = new Acceleration(0.0,
29713                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29714         calibrator.getInitialBiasXAsAcceleration(bx2);
29715         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29716         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29717         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29718         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29719         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29720         final Acceleration by2 = new Acceleration(0.0,
29721                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29722         calibrator.getInitialBiasYAsAcceleration(by2);
29723         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29724         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29725         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29726         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29727         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29728         final Acceleration bz2 = new Acceleration(0.0,
29729                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29730         calibrator.getInitialBiasZAsAcceleration(bz2);
29731         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29732         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29733         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29734         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29735         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29736         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29737         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29738         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29739         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29740         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29741         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29742         final double[] bias1 = calibrator.getInitialBias();
29743         assertArrayEquals(bias1, bias, 0.0);
29744         final double[] bias2 = new double[3];
29745         calibrator.getInitialBias(bias2);
29746         assertArrayEquals(bias1, bias2, 0.0);
29747         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29748         assertEquals(b1, ba);
29749         final Matrix b2 = new Matrix(3, 1);
29750         calibrator.getInitialBiasAsMatrix(b2);
29751         assertEquals(b1, b2);
29752         final Matrix ma1 = calibrator.getInitialMa();
29753         assertEquals(ma1, new Matrix(3, 3));
29754         final Matrix ma2 = new Matrix(3, 3);
29755         calibrator.getInitialMa(ma2);
29756         assertEquals(ma1, ma2);
29757         assertSame(calibrator.getMeasurements(), measurements);
29758         assertFalse(calibrator.isCommonAxisUsed());
29759         assertSame(calibrator.getListener(), this);
29760         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
29761         assertFalse(calibrator.isReady());
29762         assertFalse(calibrator.isRunning());
29763         assertNull(calibrator.getEstimatedBiases());
29764         assertFalse(calibrator.getEstimatedBiases(null));
29765         assertNull(calibrator.getEstimatedBiasesAsMatrix());
29766         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29767         assertNull(calibrator.getEstimatedBiasFx());
29768         assertNull(calibrator.getEstimatedBiasFy());
29769         assertNull(calibrator.getEstimatedBiasFz());
29770         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29771         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29772         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29773         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29774         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29775         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29776         assertNull(calibrator.getEstimatedMa());
29777         assertNull(calibrator.getEstimatedSx());
29778         assertNull(calibrator.getEstimatedSy());
29779         assertNull(calibrator.getEstimatedSz());
29780         assertNull(calibrator.getEstimatedMxy());
29781         assertNull(calibrator.getEstimatedMxz());
29782         assertNull(calibrator.getEstimatedMyx());
29783         assertNull(calibrator.getEstimatedMyz());
29784         assertNull(calibrator.getEstimatedMzx());
29785         assertNull(calibrator.getEstimatedMzy());
29786         assertNull(calibrator.getEstimatedCovariance());
29787         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29788         assertNotNull(calibrator.getGroundTruthGravityNorm());
29789         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29790         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29791         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29792                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29793         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29794         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29795         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29796 
29797         // Force IllegalArgumentException
29798         final Acceleration invalidGravityNorm = new Acceleration(
29799                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29800 
29801         calibrator = null;
29802         try {
29803             calibrator = new KnownGravityNormAccelerometerCalibrator(
29804                     invalidGravityNorm, measurements,
29805                     ba, this);
29806             fail("IllegalArgumentException expected but not thrown");
29807         } catch (final IllegalArgumentException ignore) {
29808         }
29809         try {
29810             calibrator = new KnownGravityNormAccelerometerCalibrator(
29811                     gravityNorm, measurements,
29812                     new Matrix(1, 1), this);
29813             fail("IllegalArgumentException expected but not thrown");
29814         } catch (final IllegalArgumentException ignore) {
29815         }
29816         try {
29817             calibrator = new KnownGravityNormAccelerometerCalibrator(
29818                     gravityNorm, measurements,
29819                     new Matrix(1, 3), this);
29820             fail("IllegalArgumentException expected but not thrown");
29821         } catch (final IllegalArgumentException ignore) {
29822         }
29823         assertNull(calibrator);
29824     }
29825 
29826     @Test
29827     public void testConstructor223() throws WrongSizeException {
29828         final Matrix ba = generateBa();
29829         final double[] bias = ba.getBuffer();
29830         final double biasX = ba.getElementAtIndex(0);
29831         final double biasY = ba.getElementAtIndex(1);
29832         final double biasZ = ba.getElementAtIndex(2);
29833 
29834         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29835         final double latitude = Math.toRadians(
29836                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29837         final double longitude = Math.toRadians(
29838                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29839         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29840         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29841         final NEDVelocity nedVelocity = new NEDVelocity();
29842         final ECEFPosition ecefPosition = new ECEFPosition();
29843         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29844         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29845                 ecefPosition, ecefVelocity);
29846         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29847                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29848         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
29849 
29850         KnownGravityNormAccelerometerCalibrator calibrator =
29851                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
29852                         true, ba);
29853 
29854         // check default values
29855         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29856         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29857         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29858         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29859         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29860         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29861         final Acceleration bx2 = new Acceleration(0.0,
29862                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29863         calibrator.getInitialBiasXAsAcceleration(bx2);
29864         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29865         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29866         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29867         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29868         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29869         final Acceleration by2 = new Acceleration(0.0,
29870                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29871         calibrator.getInitialBiasYAsAcceleration(by2);
29872         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29873         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29874         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29875         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29876         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29877         final Acceleration bz2 = new Acceleration(0.0,
29878                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29879         calibrator.getInitialBiasZAsAcceleration(bz2);
29880         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29881         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29882         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29883         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29884         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29885         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29886         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29887         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29888         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29889         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29890         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29891         final double[] bias1 = calibrator.getInitialBias();
29892         assertArrayEquals(bias1, bias, 0.0);
29893         final double[] bias2 = new double[3];
29894         calibrator.getInitialBias(bias2);
29895         assertArrayEquals(bias1, bias2, 0.0);
29896         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29897         assertEquals(b1, ba);
29898         final Matrix b2 = new Matrix(3, 1);
29899         calibrator.getInitialBiasAsMatrix(b2);
29900         assertEquals(b1, b2);
29901         final Matrix ma1 = calibrator.getInitialMa();
29902         assertEquals(ma1, new Matrix(3, 3));
29903         final Matrix ma2 = new Matrix(3, 3);
29904         calibrator.getInitialMa(ma2);
29905         assertEquals(ma1, ma2);
29906         assertNull(calibrator.getMeasurements());
29907         assertTrue(calibrator.isCommonAxisUsed());
29908         assertNull(calibrator.getListener());
29909         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
29910         assertFalse(calibrator.isReady());
29911         assertFalse(calibrator.isRunning());
29912         assertNull(calibrator.getEstimatedBiases());
29913         assertFalse(calibrator.getEstimatedBiases(null));
29914         assertNull(calibrator.getEstimatedBiasesAsMatrix());
29915         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29916         assertNull(calibrator.getEstimatedBiasFx());
29917         assertNull(calibrator.getEstimatedBiasFy());
29918         assertNull(calibrator.getEstimatedBiasFz());
29919         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29920         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29921         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29922         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29923         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29924         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29925         assertNull(calibrator.getEstimatedMa());
29926         assertNull(calibrator.getEstimatedSx());
29927         assertNull(calibrator.getEstimatedSy());
29928         assertNull(calibrator.getEstimatedSz());
29929         assertNull(calibrator.getEstimatedMxy());
29930         assertNull(calibrator.getEstimatedMxz());
29931         assertNull(calibrator.getEstimatedMyx());
29932         assertNull(calibrator.getEstimatedMyz());
29933         assertNull(calibrator.getEstimatedMzx());
29934         assertNull(calibrator.getEstimatedMzy());
29935         assertNull(calibrator.getEstimatedCovariance());
29936         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29937         assertNotNull(calibrator.getGroundTruthGravityNorm());
29938         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29939         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29940         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29941                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29942         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29943         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29944         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29945 
29946         // Force IllegalArgumentException
29947         final Acceleration invalidGravityNorm = new Acceleration(
29948                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29949 
29950         calibrator = null;
29951         try {
29952             calibrator = new KnownGravityNormAccelerometerCalibrator(
29953                     invalidGravityNorm, true, ba);
29954             fail("IllegalArgumentException expected but not thrown");
29955         } catch (final IllegalArgumentException ignore) {
29956         }
29957         try {
29958             calibrator = new KnownGravityNormAccelerometerCalibrator(
29959                     gravityNorm, true,
29960                     new Matrix(1, 1));
29961             fail("IllegalArgumentException expected but not thrown");
29962         } catch (final IllegalArgumentException ignore) {
29963         }
29964         try {
29965             calibrator = new KnownGravityNormAccelerometerCalibrator(
29966                     gravityNorm, true,
29967                     new Matrix(1, 3));
29968             fail("IllegalArgumentException expected but not thrown");
29969         } catch (final IllegalArgumentException ignore) {
29970         }
29971         assertNull(calibrator);
29972     }
29973 
29974     @Test
29975     public void testConstructor224() throws WrongSizeException {
29976         final Matrix ba = generateBa();
29977         final double[] bias = ba.getBuffer();
29978         final double biasX = ba.getElementAtIndex(0);
29979         final double biasY = ba.getElementAtIndex(1);
29980         final double biasZ = ba.getElementAtIndex(2);
29981 
29982         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29983         final double latitude = Math.toRadians(
29984                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29985         final double longitude = Math.toRadians(
29986                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29987         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29988         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29989         final NEDVelocity nedVelocity = new NEDVelocity();
29990         final ECEFPosition ecefPosition = new ECEFPosition();
29991         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29992         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29993                 ecefPosition, ecefVelocity);
29994         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29995                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29996         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
29997 
29998         KnownGravityNormAccelerometerCalibrator calibrator =
29999                 new KnownGravityNormAccelerometerCalibrator(
30000                         gravityNorm, true, ba, this);
30001 
30002         // check default values
30003         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30004         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30005         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30006         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30007         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30008         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30009         final Acceleration bx2 = new Acceleration(0.0,
30010                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30011         calibrator.getInitialBiasXAsAcceleration(bx2);
30012         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30013         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30014         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30015         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30016         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30017         final Acceleration by2 = new Acceleration(0.0,
30018                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30019         calibrator.getInitialBiasYAsAcceleration(by2);
30020         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30021         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30022         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30023         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30024         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30025         final Acceleration bz2 = new Acceleration(0.0,
30026                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30027         calibrator.getInitialBiasZAsAcceleration(bz2);
30028         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30029         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30030         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
30031         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
30032         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
30033         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
30034         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
30035         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
30036         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
30037         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
30038         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
30039         final double[] bias1 = calibrator.getInitialBias();
30040         assertArrayEquals(bias1, bias, 0.0);
30041         final double[] bias2 = new double[3];
30042         calibrator.getInitialBias(bias2);
30043         assertArrayEquals(bias1, bias2, 0.0);
30044         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30045         assertEquals(b1, ba);
30046         final Matrix b2 = new Matrix(3, 1);
30047         calibrator.getInitialBiasAsMatrix(b2);
30048         assertEquals(b1, b2);
30049         final Matrix ma1 = calibrator.getInitialMa();
30050         assertEquals(ma1, new Matrix(3, 3));
30051         final Matrix ma2 = new Matrix(3, 3);
30052         calibrator.getInitialMa(ma2);
30053         assertEquals(ma1, ma2);
30054         assertNull(calibrator.getMeasurements());
30055         assertTrue(calibrator.isCommonAxisUsed());
30056         assertSame(calibrator.getListener(), this);
30057         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30058         assertFalse(calibrator.isReady());
30059         assertFalse(calibrator.isRunning());
30060         assertNull(calibrator.getEstimatedBiases());
30061         assertFalse(calibrator.getEstimatedBiases(null));
30062         assertNull(calibrator.getEstimatedBiasesAsMatrix());
30063         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30064         assertNull(calibrator.getEstimatedBiasFx());
30065         assertNull(calibrator.getEstimatedBiasFy());
30066         assertNull(calibrator.getEstimatedBiasFz());
30067         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30068         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30069         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30070         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30071         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30072         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30073         assertNull(calibrator.getEstimatedMa());
30074         assertNull(calibrator.getEstimatedSx());
30075         assertNull(calibrator.getEstimatedSy());
30076         assertNull(calibrator.getEstimatedSz());
30077         assertNull(calibrator.getEstimatedMxy());
30078         assertNull(calibrator.getEstimatedMxz());
30079         assertNull(calibrator.getEstimatedMyx());
30080         assertNull(calibrator.getEstimatedMyz());
30081         assertNull(calibrator.getEstimatedMzx());
30082         assertNull(calibrator.getEstimatedMzy());
30083         assertNull(calibrator.getEstimatedCovariance());
30084         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30085         assertNotNull(calibrator.getGroundTruthGravityNorm());
30086         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30087         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30088         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30089                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30090         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30091         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30092         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30093 
30094         // Force IllegalArgumentException
30095         final Acceleration invalidGravityNorm = new Acceleration(
30096                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30097 
30098         calibrator = null;
30099         try {
30100             calibrator = new KnownGravityNormAccelerometerCalibrator(
30101                     invalidGravityNorm, true, ba, this);
30102             fail("IllegalArgumentException expected but not thrown");
30103         } catch (final IllegalArgumentException ignore) {
30104         }
30105         try {
30106             calibrator = new KnownGravityNormAccelerometerCalibrator(
30107                     gravityNorm, true, new Matrix(1, 1),
30108                     this);
30109             fail("IllegalArgumentException expected but not thrown");
30110         } catch (final IllegalArgumentException ignore) {
30111         }
30112         try {
30113             calibrator = new KnownGravityNormAccelerometerCalibrator(
30114                     gravityNorm, true, new Matrix(1, 3),
30115                     this);
30116             fail("IllegalArgumentException expected but not thrown");
30117         } catch (final IllegalArgumentException ignore) {
30118         }
30119         assertNull(calibrator);
30120     }
30121 
30122     @Test
30123     public void testConstructor225() throws WrongSizeException {
30124         final Collection<StandardDeviationBodyKinematics> measurements =
30125                 Collections.emptyList();
30126 
30127         final Matrix ba = generateBa();
30128         final double[] bias = ba.getBuffer();
30129         final double biasX = ba.getElementAtIndex(0);
30130         final double biasY = ba.getElementAtIndex(1);
30131         final double biasZ = ba.getElementAtIndex(2);
30132 
30133         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30134         final double latitude = Math.toRadians(
30135                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
30136         final double longitude = Math.toRadians(
30137                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30138         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30139         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30140         final NEDVelocity nedVelocity = new NEDVelocity();
30141         final ECEFPosition ecefPosition = new ECEFPosition();
30142         final ECEFVelocity ecefVelocity = new ECEFVelocity();
30143         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30144                 ecefPosition, ecefVelocity);
30145         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30146                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30147         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
30148 
30149         KnownGravityNormAccelerometerCalibrator calibrator =
30150                 new KnownGravityNormAccelerometerCalibrator(
30151                         gravityNorm, measurements,
30152                         true, ba);
30153 
30154         // check default values
30155         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30156         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30157         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30158         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30159         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30160         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30161         final Acceleration bx2 = new Acceleration(0.0,
30162                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30163         calibrator.getInitialBiasXAsAcceleration(bx2);
30164         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30165         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30166         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30167         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30168         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30169         final Acceleration by2 = new Acceleration(0.0,
30170                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30171         calibrator.getInitialBiasYAsAcceleration(by2);
30172         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30173         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30174         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30175         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30176         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30177         final Acceleration bz2 = new Acceleration(0.0,
30178                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30179         calibrator.getInitialBiasZAsAcceleration(bz2);
30180         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30181         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30182         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
30183         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
30184         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
30185         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
30186         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
30187         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
30188         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
30189         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
30190         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
30191         final double[] bias1 = calibrator.getInitialBias();
30192         assertArrayEquals(bias1, bias, 0.0);
30193         final double[] bias2 = new double[3];
30194         calibrator.getInitialBias(bias2);
30195         assertArrayEquals(bias1, bias2, 0.0);
30196         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30197         assertEquals(b1, ba);
30198         final Matrix b2 = new Matrix(3, 1);
30199         calibrator.getInitialBiasAsMatrix(b2);
30200         assertEquals(b1, b2);
30201         final Matrix ma1 = calibrator.getInitialMa();
30202         assertEquals(ma1, new Matrix(3, 3));
30203         final Matrix ma2 = new Matrix(3, 3);
30204         calibrator.getInitialMa(ma2);
30205         assertEquals(ma1, ma2);
30206         assertSame(calibrator.getMeasurements(), measurements);
30207         assertTrue(calibrator.isCommonAxisUsed());
30208         assertNull(calibrator.getListener());
30209         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30210         assertFalse(calibrator.isReady());
30211         assertFalse(calibrator.isRunning());
30212         assertNull(calibrator.getEstimatedBiases());
30213         assertFalse(calibrator.getEstimatedBiases(null));
30214         assertNull(calibrator.getEstimatedBiasesAsMatrix());
30215         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30216         assertNull(calibrator.getEstimatedBiasFx());
30217         assertNull(calibrator.getEstimatedBiasFy());
30218         assertNull(calibrator.getEstimatedBiasFz());
30219         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30220         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30221         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30222         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30223         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30224         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30225         assertNull(calibrator.getEstimatedMa());
30226         assertNull(calibrator.getEstimatedSx());
30227         assertNull(calibrator.getEstimatedSy());
30228         assertNull(calibrator.getEstimatedSz());
30229         assertNull(calibrator.getEstimatedMxy());
30230         assertNull(calibrator.getEstimatedMxz());
30231         assertNull(calibrator.getEstimatedMyx());
30232         assertNull(calibrator.getEstimatedMyz());
30233         assertNull(calibrator.getEstimatedMzx());
30234         assertNull(calibrator.getEstimatedMzy());
30235         assertNull(calibrator.getEstimatedCovariance());
30236         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30237         assertNotNull(calibrator.getGroundTruthGravityNorm());
30238         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30239         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30240         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30241                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30242         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30243         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30244         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30245 
30246         // Force IllegalArgumentException
30247         final Acceleration invalidGravityNorm = new Acceleration(
30248                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30249 
30250         calibrator = null;
30251         try {
30252             calibrator = new KnownGravityNormAccelerometerCalibrator(
30253                     invalidGravityNorm, measurements,
30254                     true, ba);
30255             fail("IllegalArgumentException expected but not thrown");
30256         } catch (final IllegalArgumentException ignore) {
30257         }
30258         try {
30259             calibrator = new KnownGravityNormAccelerometerCalibrator(
30260                     gravityNorm, measurements, true,
30261                     new Matrix(1, 1));
30262             fail("IllegalArgumentException expected but not thrown");
30263         } catch (final IllegalArgumentException ignore) {
30264         }
30265         try {
30266             calibrator = new KnownGravityNormAccelerometerCalibrator(
30267                     gravityNorm, measurements, true,
30268                     new Matrix(1, 3));
30269             fail("IllegalArgumentException expected but not thrown");
30270         } catch (final IllegalArgumentException ignore) {
30271         }
30272         assertNull(calibrator);
30273     }
30274 
30275     @Test
30276     public void testConstructor226() throws WrongSizeException {
30277         final Collection<StandardDeviationBodyKinematics> measurements =
30278                 Collections.emptyList();
30279 
30280         final Matrix ba = generateBa();
30281         final double[] bias = ba.getBuffer();
30282         final double biasX = ba.getElementAtIndex(0);
30283         final double biasY = ba.getElementAtIndex(1);
30284         final double biasZ = ba.getElementAtIndex(2);
30285 
30286         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30287         final double latitude = Math.toRadians(
30288                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
30289         final double longitude = Math.toRadians(
30290                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30291         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30292         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30293         final NEDVelocity nedVelocity = new NEDVelocity();
30294         final ECEFPosition ecefPosition = new ECEFPosition();
30295         final ECEFVelocity ecefVelocity = new ECEFVelocity();
30296         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30297                 ecefPosition, ecefVelocity);
30298         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30299                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30300         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
30301 
30302         KnownGravityNormAccelerometerCalibrator calibrator =
30303                 new KnownGravityNormAccelerometerCalibrator(
30304                         gravityNorm, measurements,
30305                         true, ba, this);
30306 
30307         // check default values
30308         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30309         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30310         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30311         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30312         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30313         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30314         final Acceleration bx2 = new Acceleration(0.0,
30315                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30316         calibrator.getInitialBiasXAsAcceleration(bx2);
30317         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30318         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30319         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30320         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30321         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30322         final Acceleration by2 = new Acceleration(0.0,
30323                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30324         calibrator.getInitialBiasYAsAcceleration(by2);
30325         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30326         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30327         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30328         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30329         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30330         final Acceleration bz2 = new Acceleration(0.0,
30331                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30332         calibrator.getInitialBiasZAsAcceleration(bz2);
30333         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30334         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30335         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
30336         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
30337         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
30338         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
30339         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
30340         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
30341         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
30342         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
30343         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
30344         final double[] bias1 = calibrator.getInitialBias();
30345         assertArrayEquals(bias1, bias, 0.0);
30346         final double[] bias2 = new double[3];
30347         calibrator.getInitialBias(bias2);
30348         assertArrayEquals(bias1, bias2, 0.0);
30349         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30350         assertEquals(b1, ba);
30351         final Matrix b2 = new Matrix(3, 1);
30352         calibrator.getInitialBiasAsMatrix(b2);
30353         assertEquals(b1, b2);
30354         final Matrix ma1 = calibrator.getInitialMa();
30355         assertEquals(ma1, new Matrix(3, 3));
30356         final Matrix ma2 = new Matrix(3, 3);
30357         calibrator.getInitialMa(ma2);
30358         assertEquals(ma1, ma2);
30359         assertSame(calibrator.getMeasurements(), measurements);
30360         assertTrue(calibrator.isCommonAxisUsed());
30361         assertSame(calibrator.getListener(), this);
30362         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30363         assertFalse(calibrator.isReady());
30364         assertFalse(calibrator.isRunning());
30365         assertNull(calibrator.getEstimatedBiases());
30366         assertFalse(calibrator.getEstimatedBiases(null));
30367         assertNull(calibrator.getEstimatedBiasesAsMatrix());
30368         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30369         assertNull(calibrator.getEstimatedBiasFx());
30370         assertNull(calibrator.getEstimatedBiasFy());
30371         assertNull(calibrator.getEstimatedBiasFz());
30372         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30373         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30374         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30375         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30376         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30377         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30378         assertNull(calibrator.getEstimatedMa());
30379         assertNull(calibrator.getEstimatedSx());
30380         assertNull(calibrator.getEstimatedSy());
30381         assertNull(calibrator.getEstimatedSz());
30382         assertNull(calibrator.getEstimatedMxy());
30383         assertNull(calibrator.getEstimatedMxz());
30384         assertNull(calibrator.getEstimatedMyx());
30385         assertNull(calibrator.getEstimatedMyz());
30386         assertNull(calibrator.getEstimatedMzx());
30387         assertNull(calibrator.getEstimatedMzy());
30388         assertNull(calibrator.getEstimatedCovariance());
30389         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30390         assertNotNull(calibrator.getGroundTruthGravityNorm());
30391         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30392         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30393         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30394                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30395         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30396         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30397         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30398 
30399         // Force IllegalArgumentException
30400         final Acceleration invalidGravityNorm = new Acceleration(
30401                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30402 
30403         calibrator = null;
30404         try {
30405             calibrator = new KnownGravityNormAccelerometerCalibrator(
30406                     invalidGravityNorm, measurements,
30407                     true, ba, this);
30408             fail("IllegalArgumentException expected but not thrown");
30409         } catch (final IllegalArgumentException ignore) {
30410         }
30411         try {
30412             calibrator = new KnownGravityNormAccelerometerCalibrator(
30413                     gravityNorm, measurements, true,
30414                     new Matrix(1, 1), this);
30415             fail("IllegalArgumentException expected but not thrown");
30416         } catch (final IllegalArgumentException ignore) {
30417         }
30418         try {
30419             calibrator = new KnownGravityNormAccelerometerCalibrator(
30420                     gravityNorm, measurements, true,
30421                     new Matrix(1, 3), this);
30422             fail("IllegalArgumentException expected but not thrown");
30423         } catch (final IllegalArgumentException ignore) {
30424         }
30425         assertNull(calibrator);
30426     }
30427 
30428     @Test
30429     public void testConstructor227() throws WrongSizeException {
30430         final Matrix ba = generateBa();
30431         final double[] bias = ba.getBuffer();
30432         final double biasX = ba.getElementAtIndex(0);
30433         final double biasY = ba.getElementAtIndex(1);
30434         final double biasZ = ba.getElementAtIndex(2);
30435 
30436         final Matrix ma = generateMaCommonAxis();
30437         final double sx = ma.getElementAt(0, 0);
30438         final double sy = ma.getElementAt(1, 1);
30439         final double sz = ma.getElementAt(2, 2);
30440         final double mxy = ma.getElementAt(0, 1);
30441         final double mxz = ma.getElementAt(0, 2);
30442         final double myx = ma.getElementAt(1, 0);
30443         final double myz = ma.getElementAt(1, 2);
30444         final double mzx = ma.getElementAt(2, 0);
30445         final double mzy = ma.getElementAt(2, 1);
30446 
30447         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30448         final double latitude = Math.toRadians(
30449                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
30450         final double longitude = Math.toRadians(
30451                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30452         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30453         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30454         final NEDVelocity nedVelocity = new NEDVelocity();
30455         final ECEFPosition ecefPosition = new ECEFPosition();
30456         final ECEFVelocity ecefVelocity = new ECEFVelocity();
30457         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30458                 ecefPosition, ecefVelocity);
30459         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30460                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30461         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
30462 
30463         KnownGravityNormAccelerometerCalibrator calibrator =
30464                 new KnownGravityNormAccelerometerCalibrator(
30465                         gravityNorm, ba, ma);
30466 
30467         // check default values
30468         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30469         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30470         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30471         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30472         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30473         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30474         final Acceleration bx2 = new Acceleration(0.0,
30475                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30476         calibrator.getInitialBiasXAsAcceleration(bx2);
30477         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30478         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30479         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30480         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30481         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30482         final Acceleration by2 = new Acceleration(0.0,
30483                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30484         calibrator.getInitialBiasYAsAcceleration(by2);
30485         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30486         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30487         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30488         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30489         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30490         final Acceleration bz2 = new Acceleration(0.0,
30491                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30492         calibrator.getInitialBiasZAsAcceleration(bz2);
30493         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30494         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30495         assertEquals(calibrator.getInitialSx(), sx, 0.0);
30496         assertEquals(calibrator.getInitialSy(), sy, 0.0);
30497         assertEquals(calibrator.getInitialSz(), sz, 0.0);
30498         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30499         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30500         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30501         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30502         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30503         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30504         final double[] bias1 = calibrator.getInitialBias();
30505         assertArrayEquals(bias1, bias, 0.0);
30506         final double[] bias2 = new double[3];
30507         calibrator.getInitialBias(bias2);
30508         assertArrayEquals(bias1, bias2, 0.0);
30509         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30510         assertEquals(b1, ba);
30511         final Matrix b2 = new Matrix(3, 1);
30512         calibrator.getInitialBiasAsMatrix(b2);
30513         assertEquals(b1, b2);
30514         final Matrix ma1 = new Matrix(3, 3);
30515         ma1.setSubmatrix(0, 0,
30516                 2, 2,
30517                 new double[]{sx, myx, mzx,
30518                         mxy, sy, mzy,
30519                         mxz, myz, sz});
30520         assertEquals(calibrator.getInitialMa(), ma1);
30521         final Matrix ma2 = new Matrix(3, 3);
30522         calibrator.getInitialMa(ma2);
30523         assertEquals(ma1, ma2);
30524         assertNull(calibrator.getMeasurements());
30525         assertFalse(calibrator.isCommonAxisUsed());
30526         assertNull(calibrator.getListener());
30527         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
30528         assertFalse(calibrator.isReady());
30529         assertFalse(calibrator.isRunning());
30530         assertNull(calibrator.getEstimatedBiases());
30531         assertFalse(calibrator.getEstimatedBiases(null));
30532         assertNull(calibrator.getEstimatedBiasesAsMatrix());
30533         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30534         assertNull(calibrator.getEstimatedBiasFx());
30535         assertNull(calibrator.getEstimatedBiasFy());
30536         assertNull(calibrator.getEstimatedBiasFz());
30537         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30538         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30539         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30540         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30541         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30542         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30543         assertNull(calibrator.getEstimatedMa());
30544         assertNull(calibrator.getEstimatedSx());
30545         assertNull(calibrator.getEstimatedSy());
30546         assertNull(calibrator.getEstimatedSz());
30547         assertNull(calibrator.getEstimatedMxy());
30548         assertNull(calibrator.getEstimatedMxz());
30549         assertNull(calibrator.getEstimatedMyx());
30550         assertNull(calibrator.getEstimatedMyz());
30551         assertNull(calibrator.getEstimatedMzx());
30552         assertNull(calibrator.getEstimatedMzy());
30553         assertNull(calibrator.getEstimatedCovariance());
30554         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30555         assertNotNull(calibrator.getGroundTruthGravityNorm());
30556         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30557         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30558         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30559                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30560         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30561         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30562         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30563 
30564         // Force IllegalArgumentException
30565         final Acceleration invalidGravityNorm = new Acceleration(
30566                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30567 
30568         calibrator = null;
30569         try {
30570             calibrator = new KnownGravityNormAccelerometerCalibrator(
30571                     invalidGravityNorm, ba, ma);
30572             fail("IllegalArgumentException expected but not thrown");
30573         } catch (final IllegalArgumentException ignore) {
30574         }
30575         try {
30576             calibrator = new KnownGravityNormAccelerometerCalibrator(
30577                     gravityNorm, new Matrix(1, 1), ma);
30578             fail("IllegalArgumentException expected but not thrown");
30579         } catch (final IllegalArgumentException ignore) {
30580         }
30581         try {
30582             calibrator = new KnownGravityNormAccelerometerCalibrator(
30583                     gravityNorm, new Matrix(1, 3), ma);
30584             fail("IllegalArgumentException expected but not thrown");
30585         } catch (final IllegalArgumentException ignore) {
30586         }
30587         try {
30588             calibrator = new KnownGravityNormAccelerometerCalibrator(
30589                     gravityNorm, ba, new Matrix(1, 3));
30590             fail("IllegalArgumentException expected but not thrown");
30591         } catch (final IllegalArgumentException ignore) {
30592         }
30593         try {
30594             calibrator = new KnownGravityNormAccelerometerCalibrator(
30595                     gravityNorm,
30596                     ba, new Matrix(3, 1));
30597             fail("IllegalArgumentException expected but not thrown");
30598         } catch (final IllegalArgumentException ignore) {
30599         }
30600         assertNull(calibrator);
30601     }
30602 
30603     @Test
30604     public void testConstructor228() throws WrongSizeException {
30605         final Matrix ba = generateBa();
30606         final double[] bias = ba.getBuffer();
30607         final double biasX = ba.getElementAtIndex(0);
30608         final double biasY = ba.getElementAtIndex(1);
30609         final double biasZ = ba.getElementAtIndex(2);
30610 
30611         final Matrix ma = generateMaCommonAxis();
30612         final double sx = ma.getElementAt(0, 0);
30613         final double sy = ma.getElementAt(1, 1);
30614         final double sz = ma.getElementAt(2, 2);
30615         final double mxy = ma.getElementAt(0, 1);
30616         final double mxz = ma.getElementAt(0, 2);
30617         final double myx = ma.getElementAt(1, 0);
30618         final double myz = ma.getElementAt(1, 2);
30619         final double mzx = ma.getElementAt(2, 0);
30620         final double mzy = ma.getElementAt(2, 1);
30621 
30622         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30623         final double latitude = Math.toRadians(
30624                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
30625         final double longitude = Math.toRadians(
30626                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30627         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30628         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30629         final NEDVelocity nedVelocity = new NEDVelocity();
30630         final ECEFPosition ecefPosition = new ECEFPosition();
30631         final ECEFVelocity ecefVelocity = new ECEFVelocity();
30632         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30633                 ecefPosition, ecefVelocity);
30634         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30635                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30636         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
30637 
30638         KnownGravityNormAccelerometerCalibrator calibrator =
30639                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
30640                         ba, ma, this);
30641 
30642         // check default values
30643         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30644         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30645         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30646         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30647         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30648         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30649         final Acceleration bx2 = new Acceleration(0.0,
30650                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30651         calibrator.getInitialBiasXAsAcceleration(bx2);
30652         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30653         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30654         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30655         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30656         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30657         final Acceleration by2 = new Acceleration(0.0,
30658                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30659         calibrator.getInitialBiasYAsAcceleration(by2);
30660         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30661         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30662         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30663         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30664         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30665         final Acceleration bz2 = new Acceleration(0.0,
30666                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30667         calibrator.getInitialBiasZAsAcceleration(bz2);
30668         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30669         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30670         assertEquals(calibrator.getInitialSx(), sx, 0.0);
30671         assertEquals(calibrator.getInitialSy(), sy, 0.0);
30672         assertEquals(calibrator.getInitialSz(), sz, 0.0);
30673         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30674         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30675         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30676         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30677         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30678         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30679         final double[] bias1 = calibrator.getInitialBias();
30680         assertArrayEquals(bias1, bias, 0.0);
30681         final double[] bias2 = new double[3];
30682         calibrator.getInitialBias(bias2);
30683         assertArrayEquals(bias1, bias2, 0.0);
30684         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30685         assertEquals(b1, ba);
30686         final Matrix b2 = new Matrix(3, 1);
30687         calibrator.getInitialBiasAsMatrix(b2);
30688         assertEquals(b1, b2);
30689         final Matrix ma1 = new Matrix(3, 3);
30690         ma1.setSubmatrix(0, 0,
30691                 2, 2,
30692                 new double[]{sx, myx, mzx,
30693                         mxy, sy, mzy,
30694                         mxz, myz, sz});
30695         assertEquals(calibrator.getInitialMa(), ma1);
30696         final Matrix ma2 = new Matrix(3, 3);
30697         calibrator.getInitialMa(ma2);
30698         assertEquals(ma1, ma2);
30699         assertNull(calibrator.getMeasurements());
30700         assertFalse(calibrator.isCommonAxisUsed());
30701         assertSame(calibrator.getListener(), this);
30702         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
30703         assertFalse(calibrator.isReady());
30704         assertFalse(calibrator.isRunning());
30705         assertNull(calibrator.getEstimatedBiases());
30706         assertFalse(calibrator.getEstimatedBiases(null));
30707         assertNull(calibrator.getEstimatedBiasesAsMatrix());
30708         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30709         assertNull(calibrator.getEstimatedBiasFx());
30710         assertNull(calibrator.getEstimatedBiasFy());
30711         assertNull(calibrator.getEstimatedBiasFz());
30712         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30713         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30714         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30715         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30716         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30717         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30718         assertNull(calibrator.getEstimatedMa());
30719         assertNull(calibrator.getEstimatedSx());
30720         assertNull(calibrator.getEstimatedSy());
30721         assertNull(calibrator.getEstimatedSz());
30722         assertNull(calibrator.getEstimatedMxy());
30723         assertNull(calibrator.getEstimatedMxz());
30724         assertNull(calibrator.getEstimatedMyx());
30725         assertNull(calibrator.getEstimatedMyz());
30726         assertNull(calibrator.getEstimatedMzx());
30727         assertNull(calibrator.getEstimatedMzy());
30728         assertNull(calibrator.getEstimatedCovariance());
30729         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30730         assertNotNull(calibrator.getGroundTruthGravityNorm());
30731         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30732         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30733         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30734                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30735         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30736         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30737         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30738 
30739         // Force IllegalArgumentException
30740         final Acceleration invalidGravityNorm = new Acceleration(
30741                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30742 
30743         calibrator = null;
30744         try {
30745             calibrator = new KnownGravityNormAccelerometerCalibrator(
30746                     invalidGravityNorm, ba, ma, this);
30747             fail("IllegalArgumentException expected but not thrown");
30748         } catch (final IllegalArgumentException ignore) {
30749         }
30750         try {
30751             calibrator = new KnownGravityNormAccelerometerCalibrator(
30752                     gravityNorm, new Matrix(1, 1), ma,
30753                     this);
30754             fail("IllegalArgumentException expected but not thrown");
30755         } catch (final IllegalArgumentException ignore) {
30756         }
30757         try {
30758             calibrator = new KnownGravityNormAccelerometerCalibrator(
30759                     gravityNorm, new Matrix(1, 3), ma,
30760                     this);
30761             fail("IllegalArgumentException expected but not thrown");
30762         } catch (final IllegalArgumentException ignore) {
30763         }
30764         try {
30765             calibrator = new KnownGravityNormAccelerometerCalibrator(
30766                     gravityNorm, ba, new Matrix(1, 3),
30767                     this);
30768             fail("IllegalArgumentException expected but not thrown");
30769         } catch (final IllegalArgumentException ignore) {
30770         }
30771         try {
30772             calibrator = new KnownGravityNormAccelerometerCalibrator(
30773                     gravityNorm, ba, new Matrix(3, 1),
30774                     this);
30775             fail("IllegalArgumentException expected but not thrown");
30776         } catch (final IllegalArgumentException ignore) {
30777         }
30778         assertNull(calibrator);
30779     }
30780 
30781     @Test
30782     public void testConstructor229() throws WrongSizeException {
30783         final Collection<StandardDeviationBodyKinematics> measurements =
30784                 Collections.emptyList();
30785 
30786         final Matrix ba = generateBa();
30787         final double[] bias = ba.getBuffer();
30788         final double biasX = ba.getElementAtIndex(0);
30789         final double biasY = ba.getElementAtIndex(1);
30790         final double biasZ = ba.getElementAtIndex(2);
30791 
30792         final Matrix ma = generateMaCommonAxis();
30793         final double sx = ma.getElementAt(0, 0);
30794         final double sy = ma.getElementAt(1, 1);
30795         final double sz = ma.getElementAt(2, 2);
30796         final double mxy = ma.getElementAt(0, 1);
30797         final double mxz = ma.getElementAt(0, 2);
30798         final double myx = ma.getElementAt(1, 0);
30799         final double myz = ma.getElementAt(1, 2);
30800         final double mzx = ma.getElementAt(2, 0);
30801         final double mzy = ma.getElementAt(2, 1);
30802 
30803         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30804         final double latitude = Math.toRadians(
30805                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
30806         final double longitude = Math.toRadians(
30807                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30808         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30809         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30810         final NEDVelocity nedVelocity = new NEDVelocity();
30811         final ECEFPosition ecefPosition = new ECEFPosition();
30812         final ECEFVelocity ecefVelocity = new ECEFVelocity();
30813         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30814                 ecefPosition, ecefVelocity);
30815         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30816                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30817         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
30818 
30819         KnownGravityNormAccelerometerCalibrator calibrator =
30820                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
30821                         measurements, ba, ma);
30822 
30823         // check default values
30824         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30825         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30826         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30827         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30828         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30829         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30830         final Acceleration bx2 = new Acceleration(0.0,
30831                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30832         calibrator.getInitialBiasXAsAcceleration(bx2);
30833         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30834         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30835         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30836         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30837         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30838         final Acceleration by2 = new Acceleration(0.0,
30839                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30840         calibrator.getInitialBiasYAsAcceleration(by2);
30841         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30842         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30843         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30844         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30845         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30846         final Acceleration bz2 = new Acceleration(0.0,
30847                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30848         calibrator.getInitialBiasZAsAcceleration(bz2);
30849         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30850         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30851         assertEquals(calibrator.getInitialSx(), sx, 0.0);
30852         assertEquals(calibrator.getInitialSy(), sy, 0.0);
30853         assertEquals(calibrator.getInitialSz(), sz, 0.0);
30854         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30855         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30856         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30857         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30858         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30859         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30860         final double[] bias1 = calibrator.getInitialBias();
30861         assertArrayEquals(bias1, bias, 0.0);
30862         final double[] bias2 = new double[3];
30863         calibrator.getInitialBias(bias2);
30864         assertArrayEquals(bias1, bias2, 0.0);
30865         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30866         assertEquals(b1, ba);
30867         final Matrix b2 = new Matrix(3, 1);
30868         calibrator.getInitialBiasAsMatrix(b2);
30869         assertEquals(b1, b2);
30870         final Matrix ma1 = new Matrix(3, 3);
30871         ma1.setSubmatrix(0, 0,
30872                 2, 2,
30873                 new double[]{sx, myx, mzx,
30874                         mxy, sy, mzy,
30875                         mxz, myz, sz});
30876         assertEquals(calibrator.getInitialMa(), ma1);
30877         final Matrix ma2 = new Matrix(3, 3);
30878         calibrator.getInitialMa(ma2);
30879         assertEquals(ma1, ma2);
30880         assertSame(calibrator.getMeasurements(), measurements);
30881         assertFalse(calibrator.isCommonAxisUsed());
30882         assertNull(calibrator.getListener());
30883         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
30884         assertFalse(calibrator.isReady());
30885         assertFalse(calibrator.isRunning());
30886         assertNull(calibrator.getEstimatedBiases());
30887         assertFalse(calibrator.getEstimatedBiases(null));
30888         assertNull(calibrator.getEstimatedBiasesAsMatrix());
30889         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30890         assertNull(calibrator.getEstimatedBiasFx());
30891         assertNull(calibrator.getEstimatedBiasFy());
30892         assertNull(calibrator.getEstimatedBiasFz());
30893         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30894         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30895         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30896         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30897         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30898         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30899         assertNull(calibrator.getEstimatedMa());
30900         assertNull(calibrator.getEstimatedSx());
30901         assertNull(calibrator.getEstimatedSy());
30902         assertNull(calibrator.getEstimatedSz());
30903         assertNull(calibrator.getEstimatedMxy());
30904         assertNull(calibrator.getEstimatedMxz());
30905         assertNull(calibrator.getEstimatedMyx());
30906         assertNull(calibrator.getEstimatedMyz());
30907         assertNull(calibrator.getEstimatedMzx());
30908         assertNull(calibrator.getEstimatedMzy());
30909         assertNull(calibrator.getEstimatedCovariance());
30910         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30911         assertNotNull(calibrator.getGroundTruthGravityNorm());
30912         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30913         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30914         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30915                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30916         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30917         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30918         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30919 
30920         // Force IllegalArgumentException
30921         final Acceleration invalidGravityNorm = new Acceleration(
30922                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30923 
30924         calibrator = null;
30925         try {
30926             calibrator = new KnownGravityNormAccelerometerCalibrator(
30927                     invalidGravityNorm, ba, ma, this);
30928             fail("IllegalArgumentException expected but not thrown");
30929         } catch (final IllegalArgumentException ignore) {
30930         }
30931         try {
30932             calibrator = new KnownGravityNormAccelerometerCalibrator(
30933                     gravityNorm, measurements,
30934                     new Matrix(1, 1), ma);
30935             fail("IllegalArgumentException expected but not thrown");
30936         } catch (final IllegalArgumentException ignore) {
30937         }
30938         try {
30939             calibrator = new KnownGravityNormAccelerometerCalibrator(
30940                     gravityNorm, measurements,
30941                     new Matrix(1, 3), ma);
30942             fail("IllegalArgumentException expected but not thrown");
30943         } catch (final IllegalArgumentException ignore) {
30944         }
30945         try {
30946             calibrator = new KnownGravityNormAccelerometerCalibrator(
30947                     gravityNorm, measurements, ba,
30948                     new Matrix(1, 3));
30949             fail("IllegalArgumentException expected but not thrown");
30950         } catch (final IllegalArgumentException ignore) {
30951         }
30952         try {
30953             calibrator = new KnownGravityNormAccelerometerCalibrator(
30954                     gravityNorm, measurements, ba,
30955                     new Matrix(3, 1));
30956             fail("IllegalArgumentException expected but not thrown");
30957         } catch (final IllegalArgumentException ignore) {
30958         }
30959         assertNull(calibrator);
30960     }
30961 
30962     @Test
30963     public void testConstructor230() throws WrongSizeException {
30964         final Collection<StandardDeviationBodyKinematics> measurements =
30965                 Collections.emptyList();
30966 
30967         final Matrix ba = generateBa();
30968         final double[] bias = ba.getBuffer();
30969         final double biasX = ba.getElementAtIndex(0);
30970         final double biasY = ba.getElementAtIndex(1);
30971         final double biasZ = ba.getElementAtIndex(2);
30972 
30973         final Matrix ma = generateMaCommonAxis();
30974         final double sx = ma.getElementAt(0, 0);
30975         final double sy = ma.getElementAt(1, 1);
30976         final double sz = ma.getElementAt(2, 2);
30977         final double mxy = ma.getElementAt(0, 1);
30978         final double mxz = ma.getElementAt(0, 2);
30979         final double myx = ma.getElementAt(1, 0);
30980         final double myz = ma.getElementAt(1, 2);
30981         final double mzx = ma.getElementAt(2, 0);
30982         final double mzy = ma.getElementAt(2, 1);
30983 
30984         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30985         final double latitude = Math.toRadians(
30986                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
30987         final double longitude = Math.toRadians(
30988                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30989         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30990         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30991         final NEDVelocity nedVelocity = new NEDVelocity();
30992         final ECEFPosition ecefPosition = new ECEFPosition();
30993         final ECEFVelocity ecefVelocity = new ECEFVelocity();
30994         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30995                 ecefPosition, ecefVelocity);
30996         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30997                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30998         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
30999 
31000         KnownGravityNormAccelerometerCalibrator calibrator =
31001                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
31002                         measurements, ba, ma, this);
31003 
31004         // check default values
31005         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
31006         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
31007         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
31008         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
31009         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
31010         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31011         final Acceleration bx2 = new Acceleration(0.0,
31012                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31013         calibrator.getInitialBiasXAsAcceleration(bx2);
31014         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
31015         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31016         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
31017         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
31018         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31019         final Acceleration by2 = new Acceleration(0.0,
31020                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31021         calibrator.getInitialBiasYAsAcceleration(by2);
31022         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
31023         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31024         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
31025         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
31026         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31027         final Acceleration bz2 = new Acceleration(0.0,
31028                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31029         calibrator.getInitialBiasZAsAcceleration(bz2);
31030         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
31031         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31032         assertEquals(calibrator.getInitialSx(), sx, 0.0);
31033         assertEquals(calibrator.getInitialSy(), sy, 0.0);
31034         assertEquals(calibrator.getInitialSz(), sz, 0.0);
31035         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31036         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31037         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31038         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31039         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31040         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31041         final double[] bias1 = calibrator.getInitialBias();
31042         assertArrayEquals(bias1, bias, 0.0);
31043         final double[] bias2 = new double[3];
31044         calibrator.getInitialBias(bias2);
31045         assertArrayEquals(bias1, bias2, 0.0);
31046         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
31047         assertEquals(b1, ba);
31048         final Matrix b2 = new Matrix(3, 1);
31049         calibrator.getInitialBiasAsMatrix(b2);
31050         assertEquals(b1, b2);
31051         final Matrix ma1 = new Matrix(3, 3);
31052         ma1.setSubmatrix(0, 0,
31053                 2, 2,
31054                 new double[]{sx, myx, mzx,
31055                         mxy, sy, mzy,
31056                         mxz, myz, sz});
31057         assertEquals(calibrator.getInitialMa(), ma1);
31058         final Matrix ma2 = new Matrix(3, 3);
31059         calibrator.getInitialMa(ma2);
31060         assertEquals(ma1, ma2);
31061         assertSame(calibrator.getMeasurements(), measurements);
31062         assertFalse(calibrator.isCommonAxisUsed());
31063         assertSame(calibrator.getListener(), this);
31064         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
31065         assertFalse(calibrator.isReady());
31066         assertFalse(calibrator.isRunning());
31067         assertNull(calibrator.getEstimatedBiases());
31068         assertFalse(calibrator.getEstimatedBiases(null));
31069         assertNull(calibrator.getEstimatedBiasesAsMatrix());
31070         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
31071         assertNull(calibrator.getEstimatedBiasFx());
31072         assertNull(calibrator.getEstimatedBiasFy());
31073         assertNull(calibrator.getEstimatedBiasFz());
31074         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
31075         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
31076         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
31077         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
31078         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
31079         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
31080         assertNull(calibrator.getEstimatedMa());
31081         assertNull(calibrator.getEstimatedSx());
31082         assertNull(calibrator.getEstimatedSy());
31083         assertNull(calibrator.getEstimatedSz());
31084         assertNull(calibrator.getEstimatedMxy());
31085         assertNull(calibrator.getEstimatedMxz());
31086         assertNull(calibrator.getEstimatedMyx());
31087         assertNull(calibrator.getEstimatedMyz());
31088         assertNull(calibrator.getEstimatedMzx());
31089         assertNull(calibrator.getEstimatedMzy());
31090         assertNull(calibrator.getEstimatedCovariance());
31091         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
31092         assertNotNull(calibrator.getGroundTruthGravityNorm());
31093         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
31094         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
31095         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
31096                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
31097         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
31098         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
31099         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
31100 
31101         // Force IllegalArgumentException
31102         final Acceleration invalidGravityNorm = new Acceleration(
31103                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31104 
31105         calibrator = null;
31106         try {
31107             calibrator = new KnownGravityNormAccelerometerCalibrator(
31108                     invalidGravityNorm, ba, ma, this);
31109             fail("IllegalArgumentException expected but not thrown");
31110         } catch (final IllegalArgumentException ignore) {
31111         }
31112         try {
31113             calibrator = new KnownGravityNormAccelerometerCalibrator(
31114                     gravityNorm, measurements,
31115                     new Matrix(1, 1), ma, this);
31116             fail("IllegalArgumentException expected but not thrown");
31117         } catch (final IllegalArgumentException ignore) {
31118         }
31119         try {
31120             calibrator = new KnownGravityNormAccelerometerCalibrator(
31121                     gravityNorm, measurements,
31122                     new Matrix(1, 3), ma, this);
31123             fail("IllegalArgumentException expected but not thrown");
31124         } catch (final IllegalArgumentException ignore) {
31125         }
31126         try {
31127             calibrator = new KnownGravityNormAccelerometerCalibrator(
31128                     gravityNorm, measurements, ba,
31129                     new Matrix(1, 3), this);
31130             fail("IllegalArgumentException expected but not thrown");
31131         } catch (final IllegalArgumentException ignore) {
31132         }
31133         try {
31134             calibrator = new KnownGravityNormAccelerometerCalibrator(
31135                     gravityNorm, measurements, ba,
31136                     new Matrix(3, 1), this);
31137             fail("IllegalArgumentException expected but not thrown");
31138         } catch (final IllegalArgumentException ignore) {
31139         }
31140         assertNull(calibrator);
31141     }
31142 
31143     @Test
31144     public void testConstructor231() throws WrongSizeException {
31145         final Matrix ba = generateBa();
31146         final double[] bias = ba.getBuffer();
31147         final double biasX = ba.getElementAtIndex(0);
31148         final double biasY = ba.getElementAtIndex(1);
31149         final double biasZ = ba.getElementAtIndex(2);
31150 
31151         final Matrix ma = generateMaCommonAxis();
31152         final double sx = ma.getElementAt(0, 0);
31153         final double sy = ma.getElementAt(1, 1);
31154         final double sz = ma.getElementAt(2, 2);
31155         final double mxy = ma.getElementAt(0, 1);
31156         final double mxz = ma.getElementAt(0, 2);
31157         final double myx = ma.getElementAt(1, 0);
31158         final double myz = ma.getElementAt(1, 2);
31159         final double mzx = ma.getElementAt(2, 0);
31160         final double mzy = ma.getElementAt(2, 1);
31161 
31162         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
31163         final double latitude = Math.toRadians(
31164                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
31165         final double longitude = Math.toRadians(
31166                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31167         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31168         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31169         final NEDVelocity nedVelocity = new NEDVelocity();
31170         final ECEFPosition ecefPosition = new ECEFPosition();
31171         final ECEFVelocity ecefVelocity = new ECEFVelocity();
31172         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
31173                 ecefPosition, ecefVelocity);
31174         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
31175                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
31176         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
31177 
31178         KnownGravityNormAccelerometerCalibrator calibrator =
31179                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
31180                         true, ba, ma);
31181 
31182         // check default values
31183         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
31184         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
31185         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
31186         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
31187         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
31188         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31189         final Acceleration bx2 = new Acceleration(0.0,
31190                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31191         calibrator.getInitialBiasXAsAcceleration(bx2);
31192         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
31193         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31194         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
31195         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
31196         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31197         final Acceleration by2 = new Acceleration(0.0,
31198                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31199         calibrator.getInitialBiasYAsAcceleration(by2);
31200         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
31201         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31202         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
31203         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
31204         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31205         final Acceleration bz2 = new Acceleration(0.0,
31206                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31207         calibrator.getInitialBiasZAsAcceleration(bz2);
31208         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
31209         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31210         assertEquals(calibrator.getInitialSx(), sx, 0.0);
31211         assertEquals(calibrator.getInitialSy(), sy, 0.0);
31212         assertEquals(calibrator.getInitialSz(), sz, 0.0);
31213         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31214         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31215         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31216         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31217         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31218         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31219         final double[] bias1 = calibrator.getInitialBias();
31220         assertArrayEquals(bias1, bias, 0.0);
31221         final double[] bias2 = new double[3];
31222         calibrator.getInitialBias(bias2);
31223         assertArrayEquals(bias1, bias2, 0.0);
31224         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
31225         assertEquals(b1, ba);
31226         final Matrix b2 = new Matrix(3, 1);
31227         calibrator.getInitialBiasAsMatrix(b2);
31228         assertEquals(b1, b2);
31229         final Matrix ma1 = new Matrix(3, 3);
31230         ma1.setSubmatrix(0, 0,
31231                 2, 2,
31232                 new double[]{sx, myx, mzx,
31233                         mxy, sy, mzy,
31234                         mxz, myz, sz});
31235         assertEquals(calibrator.getInitialMa(), ma1);
31236         final Matrix ma2 = new Matrix(3, 3);
31237         calibrator.getInitialMa(ma2);
31238         assertEquals(ma1, ma2);
31239         assertNull(calibrator.getMeasurements());
31240         assertTrue(calibrator.isCommonAxisUsed());
31241         assertNull(calibrator.getListener());
31242         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
31243         assertFalse(calibrator.isReady());
31244         assertFalse(calibrator.isRunning());
31245         assertNull(calibrator.getEstimatedBiases());
31246         assertFalse(calibrator.getEstimatedBiases(null));
31247         assertNull(calibrator.getEstimatedBiasesAsMatrix());
31248         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
31249         assertNull(calibrator.getEstimatedBiasFx());
31250         assertNull(calibrator.getEstimatedBiasFy());
31251         assertNull(calibrator.getEstimatedBiasFz());
31252         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
31253         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
31254         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
31255         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
31256         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
31257         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
31258         assertNull(calibrator.getEstimatedMa());
31259         assertNull(calibrator.getEstimatedSx());
31260         assertNull(calibrator.getEstimatedSy());
31261         assertNull(calibrator.getEstimatedSz());
31262         assertNull(calibrator.getEstimatedMxy());
31263         assertNull(calibrator.getEstimatedMxz());
31264         assertNull(calibrator.getEstimatedMyx());
31265         assertNull(calibrator.getEstimatedMyz());
31266         assertNull(calibrator.getEstimatedMzx());
31267         assertNull(calibrator.getEstimatedMzy());
31268         assertNull(calibrator.getEstimatedCovariance());
31269         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
31270         assertNotNull(calibrator.getGroundTruthGravityNorm());
31271         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
31272         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
31273         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
31274                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
31275         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
31276         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
31277         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
31278 
31279         // Force IllegalArgumentException
31280         final Acceleration invalidGravityNorm = new Acceleration(
31281                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31282 
31283         calibrator = null;
31284         try {
31285             calibrator = new KnownGravityNormAccelerometerCalibrator(
31286                     invalidGravityNorm, true, ba, ma);
31287             fail("IllegalArgumentException expected but not thrown");
31288         } catch (final IllegalArgumentException ignore) {
31289         }
31290         try {
31291             calibrator = new KnownGravityNormAccelerometerCalibrator(
31292                     gravityNorm, true,
31293                     new Matrix(1, 1), ma);
31294             fail("IllegalArgumentException expected but not thrown");
31295         } catch (final IllegalArgumentException ignore) {
31296         }
31297         try {
31298             calibrator = new KnownGravityNormAccelerometerCalibrator(
31299                     gravityNorm, true,
31300                     new Matrix(1, 3), ma);
31301             fail("IllegalArgumentException expected but not thrown");
31302         } catch (final IllegalArgumentException ignore) {
31303         }
31304         try {
31305             calibrator = new KnownGravityNormAccelerometerCalibrator(
31306                     gravityNorm, true, ba,
31307                     new Matrix(1, 3));
31308             fail("IllegalArgumentException expected but not thrown");
31309         } catch (final IllegalArgumentException ignore) {
31310         }
31311         try {
31312             calibrator = new KnownGravityNormAccelerometerCalibrator(
31313                     gravityNorm, true, ba,
31314                     new Matrix(3, 1));
31315             fail("IllegalArgumentException expected but not thrown");
31316         } catch (final IllegalArgumentException ignore) {
31317         }
31318         assertNull(calibrator);
31319     }
31320 
31321     @Test
31322     public void testConstructor232() throws WrongSizeException {
31323         final Matrix ba = generateBa();
31324         final double[] bias = ba.getBuffer();
31325         final double biasX = ba.getElementAtIndex(0);
31326         final double biasY = ba.getElementAtIndex(1);
31327         final double biasZ = ba.getElementAtIndex(2);
31328 
31329         final Matrix ma = generateMaCommonAxis();
31330         final double sx = ma.getElementAt(0, 0);
31331         final double sy = ma.getElementAt(1, 1);
31332         final double sz = ma.getElementAt(2, 2);
31333         final double mxy = ma.getElementAt(0, 1);
31334         final double mxz = ma.getElementAt(0, 2);
31335         final double myx = ma.getElementAt(1, 0);
31336         final double myz = ma.getElementAt(1, 2);
31337         final double mzx = ma.getElementAt(2, 0);
31338         final double mzy = ma.getElementAt(2, 1);
31339 
31340         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
31341         final double latitude = Math.toRadians(
31342                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
31343         final double longitude = Math.toRadians(
31344                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31345         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31346         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31347         final NEDVelocity nedVelocity = new NEDVelocity();
31348         final ECEFPosition ecefPosition = new ECEFPosition();
31349         final ECEFVelocity ecefVelocity = new ECEFVelocity();
31350         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
31351                 ecefPosition, ecefVelocity);
31352         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
31353                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
31354         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
31355 
31356         KnownGravityNormAccelerometerCalibrator calibrator =
31357                 new KnownGravityNormAccelerometerCalibrator(gravityNorm,
31358                         true, ba, ma, this);
31359 
31360         // check default values
31361         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
31362         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
31363         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
31364         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
31365         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
31366         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31367         final Acceleration bx2 = new Acceleration(0.0,
31368                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31369         calibrator.getInitialBiasXAsAcceleration(bx2);
31370         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
31371         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31372         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
31373         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
31374         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31375         final Acceleration by2 = new Acceleration(0.0,
31376                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31377         calibrator.getInitialBiasYAsAcceleration(by2);
31378         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
31379         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31380         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
31381         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
31382         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31383         final Acceleration bz2 = new Acceleration(0.0,
31384                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31385         calibrator.getInitialBiasZAsAcceleration(bz2);
31386         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
31387         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31388         assertEquals(calibrator.getInitialSx(), sx, 0.0);
31389         assertEquals(calibrator.getInitialSy(), sy, 0.0);
31390         assertEquals(calibrator.getInitialSz(), sz, 0.0);
31391         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31392         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31393         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31394         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31395         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31396         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31397         final double[] bias1 = calibrator.getInitialBias();
31398         assertArrayEquals(bias1, bias, 0.0);
31399         final double[] bias2 = new double[3];
31400         calibrator.getInitialBias(bias2);
31401         assertArrayEquals(bias1, bias2, 0.0);
31402         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
31403         assertEquals(b1, ba);
31404         final Matrix b2 = new Matrix(3, 1);
31405         calibrator.getInitialBiasAsMatrix(b2);
31406         assertEquals(b1, b2);
31407         final Matrix ma1 = new Matrix(3, 3);
31408         ma1.setSubmatrix(0, 0,
31409                 2, 2,
31410                 new double[]{sx, myx, mzx,
31411                         mxy, sy, mzy,
31412                         mxz, myz, sz});
31413         assertEquals(calibrator.getInitialMa(), ma1);
31414         final Matrix ma2 = new Matrix(3, 3);
31415         calibrator.getInitialMa(ma2);
31416         assertEquals(ma1, ma2);
31417         assertNull(calibrator.getMeasurements());
31418         assertTrue(calibrator.isCommonAxisUsed());
31419         assertSame(calibrator.getListener(), this);
31420         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
31421         assertFalse(calibrator.isReady());
31422         assertFalse(calibrator.isRunning());
31423         assertNull(calibrator.getEstimatedBiases());
31424         assertFalse(calibrator.getEstimatedBiases(null));
31425         assertNull(calibrator.getEstimatedBiasesAsMatrix());
31426         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
31427         assertNull(calibrator.getEstimatedBiasFx());
31428         assertNull(calibrator.getEstimatedBiasFy());
31429         assertNull(calibrator.getEstimatedBiasFz());
31430         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
31431         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
31432         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
31433         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
31434         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
31435         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
31436         assertNull(calibrator.getEstimatedMa());
31437         assertNull(calibrator.getEstimatedSx());
31438         assertNull(calibrator.getEstimatedSy());
31439         assertNull(calibrator.getEstimatedSz());
31440         assertNull(calibrator.getEstimatedMxy());
31441         assertNull(calibrator.getEstimatedMxz());
31442         assertNull(calibrator.getEstimatedMyx());
31443         assertNull(calibrator.getEstimatedMyz());
31444         assertNull(calibrator.getEstimatedMzx());
31445         assertNull(calibrator.getEstimatedMzy());
31446         assertNull(calibrator.getEstimatedCovariance());
31447         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
31448         assertNotNull(calibrator.getGroundTruthGravityNorm());
31449         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
31450         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
31451         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
31452                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
31453         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
31454         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
31455         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
31456 
31457         // Force IllegalArgumentException
31458         final Acceleration invalidGravityNorm = new Acceleration(
31459                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31460 
31461         calibrator = null;
31462         try {
31463             calibrator = new KnownGravityNormAccelerometerCalibrator(
31464                     invalidGravityNorm, true, ba, ma);
31465             fail("IllegalArgumentException expected but not thrown");
31466         } catch (final IllegalArgumentException ignore) {
31467         }
31468         try {
31469             calibrator = new KnownGravityNormAccelerometerCalibrator(
31470                     gravityNorm, true,
31471                     new Matrix(1, 1), ma, this);
31472             fail("IllegalArgumentException expected but not thrown");
31473         } catch (final IllegalArgumentException ignore) {
31474         }
31475         try {
31476             calibrator = new KnownGravityNormAccelerometerCalibrator(
31477                     gravityNorm, true,
31478                     new Matrix(1, 3), ma, this);
31479             fail("IllegalArgumentException expected but not thrown");
31480         } catch (final IllegalArgumentException ignore) {
31481         }
31482         try {
31483             calibrator = new KnownGravityNormAccelerometerCalibrator(
31484                     gravityNorm, true, ba,
31485                     new Matrix(1, 3), this);
31486             fail("IllegalArgumentException expected but not thrown");
31487         } catch (final IllegalArgumentException ignore) {
31488         }
31489         try {
31490             calibrator = new KnownGravityNormAccelerometerCalibrator(
31491                     gravityNorm, true, ba,
31492                     new Matrix(3, 1), this);
31493             fail("IllegalArgumentException expected but not thrown");
31494         } catch (final IllegalArgumentException ignore) {
31495         }
31496         assertNull(calibrator);
31497     }
31498 
31499     @Test
31500     public void testConstructor233() throws WrongSizeException {
31501         final Collection<StandardDeviationBodyKinematics> measurements =
31502                 Collections.emptyList();
31503 
31504         final Matrix ba = generateBa();
31505         final double[] bias = ba.getBuffer();
31506         final double biasX = ba.getElementAtIndex(0);
31507         final double biasY = ba.getElementAtIndex(1);
31508         final double biasZ = ba.getElementAtIndex(2);
31509 
31510         final Matrix ma = generateMaCommonAxis();
31511         final double sx = ma.getElementAt(0, 0);
31512         final double sy = ma.getElementAt(1, 1);
31513         final double sz = ma.getElementAt(2, 2);
31514         final double mxy = ma.getElementAt(0, 1);
31515         final double mxz = ma.getElementAt(0, 2);
31516         final double myx = ma.getElementAt(1, 0);
31517         final double myz = ma.getElementAt(1, 2);
31518         final double mzx = ma.getElementAt(2, 0);
31519         final double mzy = ma.getElementAt(2, 1);
31520 
31521         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
31522         final double latitude = Math.toRadians(
31523                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
31524         final double longitude = Math.toRadians(
31525                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31526         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31527         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31528         final NEDVelocity nedVelocity = new NEDVelocity();
31529         final ECEFPosition ecefPosition = new ECEFPosition();
31530         final ECEFVelocity ecefVelocity = new ECEFVelocity();
31531         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
31532                 ecefPosition, ecefVelocity);
31533         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
31534                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
31535         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
31536 
31537         KnownGravityNormAccelerometerCalibrator calibrator =
31538                 new KnownGravityNormAccelerometerCalibrator(
31539                         gravityNorm, measurements, true,
31540                         ba, ma);
31541 
31542         // check default values
31543         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
31544         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
31545         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
31546         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
31547         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
31548         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31549         final Acceleration bx2 = new Acceleration(0.0,
31550                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31551         calibrator.getInitialBiasXAsAcceleration(bx2);
31552         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
31553         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31554         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
31555         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
31556         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31557         final Acceleration by2 = new Acceleration(0.0,
31558                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31559         calibrator.getInitialBiasYAsAcceleration(by2);
31560         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
31561         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31562         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
31563         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
31564         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31565         final Acceleration bz2 = new Acceleration(0.0,
31566                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31567         calibrator.getInitialBiasZAsAcceleration(bz2);
31568         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
31569         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31570         assertEquals(calibrator.getInitialSx(), sx, 0.0);
31571         assertEquals(calibrator.getInitialSy(), sy, 0.0);
31572         assertEquals(calibrator.getInitialSz(), sz, 0.0);
31573         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31574         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31575         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31576         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31577         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31578         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31579         final double[] bias1 = calibrator.getInitialBias();
31580         assertArrayEquals(bias1, bias, 0.0);
31581         final double[] bias2 = new double[3];
31582         calibrator.getInitialBias(bias2);
31583         assertArrayEquals(bias1, bias2, 0.0);
31584         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
31585         assertEquals(b1, ba);
31586         final Matrix b2 = new Matrix(3, 1);
31587         calibrator.getInitialBiasAsMatrix(b2);
31588         assertEquals(b1, b2);
31589         final Matrix ma1 = new Matrix(3, 3);
31590         ma1.setSubmatrix(0, 0,
31591                 2, 2,
31592                 new double[]{sx, myx, mzx,
31593                         mxy, sy, mzy,
31594                         mxz, myz, sz});
31595         assertEquals(calibrator.getInitialMa(), ma1);
31596         final Matrix ma2 = new Matrix(3, 3);
31597         calibrator.getInitialMa(ma2);
31598         assertEquals(ma1, ma2);
31599         assertSame(calibrator.getMeasurements(), measurements);
31600         assertTrue(calibrator.isCommonAxisUsed());
31601         assertNull(calibrator.getListener());
31602         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
31603         assertFalse(calibrator.isReady());
31604         assertFalse(calibrator.isRunning());
31605         assertNull(calibrator.getEstimatedBiases());
31606         assertFalse(calibrator.getEstimatedBiases(null));
31607         assertNull(calibrator.getEstimatedBiasesAsMatrix());
31608         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
31609         assertNull(calibrator.getEstimatedBiasFx());
31610         assertNull(calibrator.getEstimatedBiasFy());
31611         assertNull(calibrator.getEstimatedBiasFz());
31612         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
31613         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
31614         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
31615         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
31616         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
31617         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
31618         assertNull(calibrator.getEstimatedMa());
31619         assertNull(calibrator.getEstimatedSx());
31620         assertNull(calibrator.getEstimatedSy());
31621         assertNull(calibrator.getEstimatedSz());
31622         assertNull(calibrator.getEstimatedMxy());
31623         assertNull(calibrator.getEstimatedMxz());
31624         assertNull(calibrator.getEstimatedMyx());
31625         assertNull(calibrator.getEstimatedMyz());
31626         assertNull(calibrator.getEstimatedMzx());
31627         assertNull(calibrator.getEstimatedMzy());
31628         assertNull(calibrator.getEstimatedCovariance());
31629         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
31630         assertNotNull(calibrator.getGroundTruthGravityNorm());
31631         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
31632         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
31633         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
31634                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
31635         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
31636         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
31637         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
31638 
31639         // Force IllegalArgumentException
31640         final Acceleration invalidGravityNorm = new Acceleration(
31641                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31642 
31643         calibrator = null;
31644         try {
31645             calibrator = new KnownGravityNormAccelerometerCalibrator(
31646                             invalidGravityNorm, measurements, true,
31647                             ba, ma);
31648             fail("IllegalArgumentException expected but not thrown");
31649         } catch (final IllegalArgumentException ignore) {
31650         }
31651         try {
31652             calibrator = new KnownGravityNormAccelerometerCalibrator(
31653                     gravityNorm, measurements, true,
31654                     new Matrix(1, 1), ma);
31655             fail("IllegalArgumentException expected but not thrown");
31656         } catch (final IllegalArgumentException ignore) {
31657         }
31658         try {
31659             calibrator = new KnownGravityNormAccelerometerCalibrator(
31660                     gravityNorm, measurements, true,
31661                     new Matrix(1, 3), ma);
31662             fail("IllegalArgumentException expected but not thrown");
31663         } catch (final IllegalArgumentException ignore) {
31664         }
31665         try {
31666             calibrator = new KnownGravityNormAccelerometerCalibrator(
31667                     gravityNorm, measurements, true,
31668                     ba, new Matrix(1, 3));
31669             fail("IllegalArgumentException expected but not thrown");
31670         } catch (final IllegalArgumentException ignore) {
31671         }
31672         try {
31673             calibrator = new KnownGravityNormAccelerometerCalibrator(
31674                     gravityNorm, measurements, true,
31675                     ba, new Matrix(3, 1));
31676             fail("IllegalArgumentException expected but not thrown");
31677         } catch (final IllegalArgumentException ignore) {
31678         }
31679         assertNull(calibrator);
31680     }
31681 
31682     @Test
31683     public void testConstructor234() throws WrongSizeException {
31684         final Collection<StandardDeviationBodyKinematics> measurements =
31685                 Collections.emptyList();
31686 
31687         final Matrix ba = generateBa();
31688         final double[] bias = ba.getBuffer();
31689         final double biasX = ba.getElementAtIndex(0);
31690         final double biasY = ba.getElementAtIndex(1);
31691         final double biasZ = ba.getElementAtIndex(2);
31692 
31693         final Matrix ma = generateMaCommonAxis();
31694         final double sx = ma.getElementAt(0, 0);
31695         final double sy = ma.getElementAt(1, 1);
31696         final double sz = ma.getElementAt(2, 2);
31697         final double mxy = ma.getElementAt(0, 1);
31698         final double mxz = ma.getElementAt(0, 2);
31699         final double myx = ma.getElementAt(1, 0);
31700         final double myz = ma.getElementAt(1, 2);
31701         final double mzx = ma.getElementAt(2, 0);
31702         final double mzy = ma.getElementAt(2, 1);
31703 
31704         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
31705         final double latitude = Math.toRadians(
31706                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
31707         final double longitude = Math.toRadians(
31708                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31709         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31710         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31711         final NEDVelocity nedVelocity = new NEDVelocity();
31712         final ECEFPosition ecefPosition = new ECEFPosition();
31713         final ECEFVelocity ecefVelocity = new ECEFVelocity();
31714         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
31715                 ecefPosition, ecefVelocity);
31716         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
31717                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
31718         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
31719 
31720         KnownGravityNormAccelerometerCalibrator calibrator =
31721                 new KnownGravityNormAccelerometerCalibrator(
31722                         gravityNorm, measurements,
31723                         true, ba, ma, this);
31724 
31725         // check default values
31726         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
31727         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
31728         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
31729         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
31730         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
31731         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31732         final Acceleration bx2 = new Acceleration(0.0,
31733                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31734         calibrator.getInitialBiasXAsAcceleration(bx2);
31735         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
31736         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31737         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
31738         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
31739         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31740         final Acceleration by2 = new Acceleration(0.0,
31741                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31742         calibrator.getInitialBiasYAsAcceleration(by2);
31743         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
31744         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31745         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
31746         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
31747         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31748         final Acceleration bz2 = new Acceleration(0.0,
31749                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
31750         calibrator.getInitialBiasZAsAcceleration(bz2);
31751         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
31752         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31753         assertEquals(calibrator.getInitialSx(), sx, 0.0);
31754         assertEquals(calibrator.getInitialSy(), sy, 0.0);
31755         assertEquals(calibrator.getInitialSz(), sz, 0.0);
31756         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31757         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31758         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31759         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31760         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31761         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31762         final double[] bias1 = calibrator.getInitialBias();
31763         assertArrayEquals(bias1, bias, 0.0);
31764         final double[] bias2 = new double[3];
31765         calibrator.getInitialBias(bias2);
31766         assertArrayEquals(bias1, bias2, 0.0);
31767         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
31768         assertEquals(b1, ba);
31769         final Matrix b2 = new Matrix(3, 1);
31770         calibrator.getInitialBiasAsMatrix(b2);
31771         assertEquals(b1, b2);
31772         final Matrix ma1 = new Matrix(3, 3);
31773         ma1.setSubmatrix(0, 0,
31774                 2, 2,
31775                 new double[]{sx, myx, mzx,
31776                         mxy, sy, mzy,
31777                         mxz, myz, sz});
31778         assertEquals(calibrator.getInitialMa(), ma1);
31779         final Matrix ma2 = new Matrix(3, 3);
31780         calibrator.getInitialMa(ma2);
31781         assertEquals(ma1, ma2);
31782         assertSame(calibrator.getMeasurements(), measurements);
31783         assertTrue(calibrator.isCommonAxisUsed());
31784         assertSame(calibrator.getListener(), this);
31785         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
31786         assertFalse(calibrator.isReady());
31787         assertFalse(calibrator.isRunning());
31788         assertNull(calibrator.getEstimatedBiases());
31789         assertFalse(calibrator.getEstimatedBiases(null));
31790         assertNull(calibrator.getEstimatedBiasesAsMatrix());
31791         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
31792         assertNull(calibrator.getEstimatedBiasFx());
31793         assertNull(calibrator.getEstimatedBiasFy());
31794         assertNull(calibrator.getEstimatedBiasFz());
31795         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
31796         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
31797         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
31798         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
31799         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
31800         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
31801         assertNull(calibrator.getEstimatedMa());
31802         assertNull(calibrator.getEstimatedSx());
31803         assertNull(calibrator.getEstimatedSy());
31804         assertNull(calibrator.getEstimatedSz());
31805         assertNull(calibrator.getEstimatedMxy());
31806         assertNull(calibrator.getEstimatedMxz());
31807         assertNull(calibrator.getEstimatedMyx());
31808         assertNull(calibrator.getEstimatedMyz());
31809         assertNull(calibrator.getEstimatedMzx());
31810         assertNull(calibrator.getEstimatedMzy());
31811         assertNull(calibrator.getEstimatedCovariance());
31812         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
31813         assertNotNull(calibrator.getGroundTruthGravityNorm());
31814         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
31815         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
31816         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
31817                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
31818         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
31819         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
31820         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
31821 
31822         // Force IllegalArgumentException
31823         final Acceleration invalidGravityNorm = new Acceleration(
31824                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31825 
31826         calibrator = null;
31827         try {
31828             calibrator = new KnownGravityNormAccelerometerCalibrator(
31829                             invalidGravityNorm, measurements,
31830                             true, ba, ma, this);
31831             fail("IllegalArgumentException expected but not thrown");
31832         } catch (final IllegalArgumentException ignore) {
31833         }
31834         try {
31835             calibrator = new KnownGravityNormAccelerometerCalibrator(
31836                     gravityNorm, measurements, true,
31837                     new Matrix(1, 1), ma, this);
31838             fail("IllegalArgumentException expected but not thrown");
31839         } catch (final IllegalArgumentException ignore) {
31840         }
31841         try {
31842             calibrator = new KnownGravityNormAccelerometerCalibrator(
31843                     gravityNorm, measurements, true,
31844                     new Matrix(1, 3), ma, this);
31845             fail("IllegalArgumentException expected but not thrown");
31846         } catch (final IllegalArgumentException ignore) {
31847         }
31848         try {
31849             calibrator = new KnownGravityNormAccelerometerCalibrator(
31850                     gravityNorm, measurements, true,
31851                     ba, new Matrix(1, 3), this);
31852             fail("IllegalArgumentException expected but not thrown");
31853         } catch (final IllegalArgumentException ignore) {
31854         }
31855         try {
31856             calibrator = new KnownGravityNormAccelerometerCalibrator(
31857                     gravityNorm, measurements, true,
31858                     ba, new Matrix(3, 1), this);
31859             fail("IllegalArgumentException expected but not thrown");
31860         } catch (final IllegalArgumentException ignore) {
31861         }
31862         assertNull(calibrator);
31863     }
31864 
31865     @Test
31866     public void testGetSetInitialBiasX() throws LockedException {
31867         final KnownGravityNormAccelerometerCalibrator calibrator =
31868                 new KnownGravityNormAccelerometerCalibrator();
31869 
31870         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
31871 
31872         // set new value
31873         final Matrix ba = generateBa();
31874         final double biasX = ba.getElementAtIndex(0);
31875 
31876         calibrator.setInitialBiasX(biasX);
31877 
31878         // check
31879         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
31880     }
31881 
31882     @Test
31883     public void testGetSetInitialBiasY() throws LockedException {
31884         final KnownGravityNormAccelerometerCalibrator calibrator =
31885                 new KnownGravityNormAccelerometerCalibrator();
31886 
31887         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
31888 
31889         // set new value
31890         final Matrix ba = generateBa();
31891         final double biasY = ba.getElementAtIndex(1);
31892 
31893         calibrator.setInitialBiasY(biasY);
31894 
31895         // check
31896         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
31897     }
31898 
31899     @Test
31900     public void testGetSetInitialBiasZ() throws LockedException {
31901         final KnownGravityNormAccelerometerCalibrator calibrator =
31902                 new KnownGravityNormAccelerometerCalibrator();
31903 
31904         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
31905 
31906         // set new value
31907         final Matrix ba = generateBa();
31908         final double biasZ = ba.getElementAtIndex(2);
31909 
31910         calibrator.setInitialBiasZ(biasZ);
31911 
31912         // check
31913         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
31914     }
31915 
31916     @Test
31917     public void testGetSetInitialBiasXAsAcceleration() throws LockedException {
31918         final KnownGravityNormAccelerometerCalibrator calibrator =
31919                 new KnownGravityNormAccelerometerCalibrator();
31920 
31921         // check default value
31922         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
31923 
31924         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
31925         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31926 
31927         // set new value
31928         final Matrix ba = generateBa();
31929         final double biasX = ba.getElementAtIndex(0);
31930 
31931         final Acceleration bx2 = new Acceleration(biasX,
31932                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31933         calibrator.setInitialBiasX(bx2);
31934 
31935         // check
31936         final Acceleration bx3 = calibrator.getInitialBiasXAsAcceleration();
31937         final Acceleration bx4 = new Acceleration(biasX,
31938                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31939         calibrator.getInitialBiasXAsAcceleration(bx4);
31940 
31941         assertEquals(bx3, bx4);
31942     }
31943 
31944     @Test
31945     public void testGetSetInitialBiasYAsAcceleration() throws LockedException {
31946         final KnownGravityNormAccelerometerCalibrator calibrator =
31947                 new KnownGravityNormAccelerometerCalibrator();
31948 
31949         // check default value
31950         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
31951 
31952         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
31953         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31954 
31955         // set new value
31956         final Matrix ba = generateBa();
31957         final double biasY = ba.getElementAtIndex(1);
31958 
31959         final Acceleration by2 = new Acceleration(biasY,
31960                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31961         calibrator.setInitialBiasY(by2);
31962 
31963         // check
31964         final Acceleration by3 = calibrator.getInitialBiasYAsAcceleration();
31965         final Acceleration by4 = new Acceleration(biasY,
31966                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31967         calibrator.getInitialBiasYAsAcceleration(by4);
31968 
31969         assertEquals(by3, by4);
31970     }
31971 
31972     @Test
31973     public void testGetSetInitialBiasZAsAcceleration() throws LockedException {
31974         final KnownGravityNormAccelerometerCalibrator calibrator =
31975                 new KnownGravityNormAccelerometerCalibrator();
31976 
31977         // check default value
31978         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
31979 
31980         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
31981         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
31982 
31983         // set new value
31984         final Matrix ba = generateBa();
31985         final double biasZ = ba.getElementAtIndex(2);
31986 
31987         final Acceleration bz2 = new Acceleration(biasZ,
31988                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31989         calibrator.setInitialBiasZ(bz2);
31990 
31991         // check
31992         final Acceleration bz3 = calibrator.getInitialBiasZAsAcceleration();
31993         final Acceleration bz4 = new Acceleration(biasZ,
31994                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31995         calibrator.getInitialBiasZAsAcceleration(bz4);
31996 
31997         assertEquals(bz3, bz4);
31998     }
31999 
32000     @Test
32001     public void testSetInitialBias1() throws LockedException {
32002         final KnownGravityNormAccelerometerCalibrator calibrator =
32003                 new KnownGravityNormAccelerometerCalibrator();
32004 
32005         // check default values
32006         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
32007         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
32008         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
32009 
32010         // set new values
32011         final Matrix ba = generateBa();
32012         final double biasX = ba.getElementAtIndex(0);
32013         final double biasY = ba.getElementAtIndex(1);
32014         final double biasZ = ba.getElementAtIndex(2);
32015 
32016         calibrator.setInitialBias(biasX, biasY, biasZ);
32017 
32018         // check
32019         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
32020         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
32021         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
32022     }
32023 
32024     @Test
32025     public void testSetInitialBias2() throws LockedException {
32026         final KnownGravityNormAccelerometerCalibrator calibrator =
32027                 new KnownGravityNormAccelerometerCalibrator();
32028 
32029         // check default values
32030         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
32031         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
32032         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
32033 
32034         // set new values
32035         final Matrix ba = generateBa();
32036         final double biasX = ba.getElementAtIndex(0);
32037         final double biasY = ba.getElementAtIndex(1);
32038         final double biasZ = ba.getElementAtIndex(2);
32039 
32040         final Acceleration bx = new Acceleration(biasX,
32041                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
32042         final Acceleration by = new Acceleration(biasY,
32043                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
32044         final Acceleration bz = new Acceleration(biasZ,
32045                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
32046 
32047         calibrator.setInitialBias(bx, by, bz);
32048 
32049         // check
32050         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
32051         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
32052         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
32053     }
32054 
32055     @Test
32056     public void testGetSetInitialSx() throws WrongSizeException, LockedException {
32057         final KnownGravityNormAccelerometerCalibrator calibrator =
32058                 new KnownGravityNormAccelerometerCalibrator();
32059 
32060         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
32061 
32062         // set new value
32063         final Matrix ma = generateMaGeneral();
32064         final double sx = ma.getElementAt(0, 0);
32065 
32066         calibrator.setInitialSx(sx);
32067 
32068         // check
32069         assertEquals(calibrator.getInitialSx(), sx, 0.0);
32070     }
32071 
32072     @Test
32073     public void testGetSetInitialSy() throws WrongSizeException, LockedException {
32074         final KnownGravityNormAccelerometerCalibrator calibrator =
32075                 new KnownGravityNormAccelerometerCalibrator();
32076 
32077         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
32078 
32079         // set new value
32080         final Matrix ma = generateMaGeneral();
32081         final double sy = ma.getElementAt(1, 1);
32082 
32083         calibrator.setInitialSy(sy);
32084 
32085         // check
32086         assertEquals(calibrator.getInitialSy(), sy, 0.0);
32087     }
32088 
32089     @Test
32090     public void testGetSetInitialSz() throws WrongSizeException, LockedException {
32091         final KnownGravityNormAccelerometerCalibrator calibrator =
32092                 new KnownGravityNormAccelerometerCalibrator();
32093 
32094         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
32095 
32096         // set new value
32097         final Matrix ma = generateMaGeneral();
32098         final double sz = ma.getElementAt(2, 2);
32099 
32100         calibrator.setInitialSz(sz);
32101 
32102         // check
32103         assertEquals(calibrator.getInitialSz(), sz, 0.0);
32104     }
32105 
32106     @Test
32107     public void testGetSetInitialMxy() throws WrongSizeException, LockedException {
32108         final KnownGravityNormAccelerometerCalibrator calibrator =
32109                 new KnownGravityNormAccelerometerCalibrator();
32110 
32111         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
32112 
32113         // set new value
32114         final Matrix ma = generateMaGeneral();
32115         final double mxy = ma.getElementAt(0, 1);
32116 
32117         calibrator.setInitialMxy(mxy);
32118 
32119         // check
32120         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
32121     }
32122 
32123     @Test
32124     public void testGetSetInitialMxz() throws WrongSizeException, LockedException {
32125         final KnownGravityNormAccelerometerCalibrator calibrator =
32126                 new KnownGravityNormAccelerometerCalibrator();
32127 
32128         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
32129 
32130         // set new value
32131         final Matrix ma = generateMaGeneral();
32132         final double mxz = ma.getElementAt(0, 2);
32133 
32134         calibrator.setInitialMxz(mxz);
32135 
32136         // check
32137         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
32138     }
32139 
32140     @Test
32141     public void testGetSetInitialMyx() throws WrongSizeException, LockedException {
32142         final KnownGravityNormAccelerometerCalibrator calibrator =
32143                 new KnownGravityNormAccelerometerCalibrator();
32144 
32145         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
32146 
32147         // set new value
32148         final Matrix ma = generateMaGeneral();
32149         final double myx = ma.getElementAt(1, 0);
32150 
32151         calibrator.setInitialMyx(myx);
32152 
32153         // check
32154         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
32155     }
32156 
32157     @Test
32158     public void testGetSetInitialMyz() throws WrongSizeException, LockedException {
32159         final KnownGravityNormAccelerometerCalibrator calibrator =
32160                 new KnownGravityNormAccelerometerCalibrator();
32161 
32162         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
32163 
32164         // set new value
32165         final Matrix ma = generateMaGeneral();
32166         final double myz = ma.getElementAt(1, 2);
32167 
32168         calibrator.setInitialMyz(myz);
32169 
32170         // check
32171         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
32172     }
32173 
32174     @Test
32175     public void testGetSetInitialMzx() throws WrongSizeException, LockedException {
32176         final KnownGravityNormAccelerometerCalibrator calibrator =
32177                 new KnownGravityNormAccelerometerCalibrator();
32178 
32179         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
32180 
32181         // set new value
32182         final Matrix ma = generateMaGeneral();
32183         final double mzx = ma.getElementAt(2, 0);
32184 
32185         calibrator.setInitialMzx(mzx);
32186 
32187         // check
32188         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
32189     }
32190 
32191     @Test
32192     public void testGetSetInitialMzy() throws WrongSizeException, LockedException {
32193         final KnownGravityNormAccelerometerCalibrator calibrator =
32194                 new KnownGravityNormAccelerometerCalibrator();
32195 
32196         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
32197 
32198         // set new value
32199         final Matrix ma = generateMaGeneral();
32200         final double mzy = ma.getElementAt(2, 1);
32201 
32202         calibrator.setInitialMzy(mzy);
32203 
32204         // check
32205         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
32206     }
32207 
32208     @Test
32209     public void testSetInitialScalingFactors()
32210             throws WrongSizeException, LockedException {
32211 
32212         final KnownGravityNormAccelerometerCalibrator calibrator =
32213                 new KnownGravityNormAccelerometerCalibrator();
32214 
32215         // check default values
32216         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
32217         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
32218         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
32219 
32220         // set new values
32221         final Matrix ma = generateMaGeneral();
32222         final double sx = ma.getElementAt(0, 0);
32223         final double sy = ma.getElementAt(1, 1);
32224         final double sz = ma.getElementAt(2, 2);
32225 
32226         calibrator.setInitialScalingFactors(sx, sy, sz);
32227 
32228         // check
32229         assertEquals(calibrator.getInitialSx(), sx, 0.0);
32230         assertEquals(calibrator.getInitialSy(), sy, 0.0);
32231         assertEquals(calibrator.getInitialSz(), sz, 0.0);
32232     }
32233 
32234     @Test
32235     public void testSetInitialCrossCouplingErrors()
32236             throws WrongSizeException, LockedException {
32237 
32238         final KnownGravityNormAccelerometerCalibrator calibrator =
32239                 new KnownGravityNormAccelerometerCalibrator();
32240 
32241         // check default values
32242         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
32243         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
32244         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
32245         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
32246         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
32247         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
32248 
32249         // set new values
32250         final Matrix ma = generateMaGeneral();
32251         final double mxy = ma.getElementAt(0, 1);
32252         final double mxz = ma.getElementAt(0, 2);
32253         final double myx = ma.getElementAt(1, 0);
32254         final double myz = ma.getElementAt(1, 2);
32255         final double mzx = ma.getElementAt(2, 0);
32256         final double mzy = ma.getElementAt(2, 1);
32257 
32258         calibrator.setInitialCrossCouplingErrors(mxy, mxz, myx, myz, mzx, mzy);
32259 
32260         // check
32261         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
32262         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
32263         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
32264         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
32265         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
32266         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
32267     }
32268 
32269     @Test
32270     public void testSetInitialScalingFactorsAndCrossCouplingErrors()
32271             throws WrongSizeException, LockedException {
32272 
32273         final KnownGravityNormAccelerometerCalibrator calibrator =
32274                 new KnownGravityNormAccelerometerCalibrator();
32275 
32276         // check default values
32277         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
32278         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
32279         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
32280         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
32281         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
32282         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
32283         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
32284         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
32285         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
32286 
32287         // set new values
32288         final Matrix ma = generateMaGeneral();
32289         final double sx = ma.getElementAt(0, 0);
32290         final double sy = ma.getElementAt(1, 1);
32291         final double sz = ma.getElementAt(2, 2);
32292         final double mxy = ma.getElementAt(0, 1);
32293         final double mxz = ma.getElementAt(0, 2);
32294         final double myx = ma.getElementAt(1, 0);
32295         final double myz = ma.getElementAt(1, 2);
32296         final double mzx = ma.getElementAt(2, 0);
32297         final double mzy = ma.getElementAt(2, 1);
32298 
32299         calibrator.setInitialScalingFactorsAndCrossCouplingErrors(sx, sy, sz,
32300                 mxy, mxz, myx, myz, mzx, mzy);
32301 
32302         // check
32303         assertEquals(calibrator.getInitialSx(), sx, 0.0);
32304         assertEquals(calibrator.getInitialSy(), sy, 0.0);
32305         assertEquals(calibrator.getInitialSz(), sz, 0.0);
32306         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
32307         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
32308         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
32309         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
32310         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
32311         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
32312     }
32313 
32314     @Test
32315     public void testGetSetInitialBias() throws LockedException {
32316         final KnownGravityNormAccelerometerCalibrator calibrator =
32317                 new KnownGravityNormAccelerometerCalibrator();
32318 
32319         // check default value
32320         final double[] bias1 = calibrator.getInitialBias();
32321         assertArrayEquals(bias1, new double[3], 0.0);
32322 
32323         // set new values
32324         final Matrix ba = generateBa();
32325         final double[] bias2 = ba.getBuffer();
32326         calibrator.setInitialBias(bias2);
32327 
32328         // check
32329         final double[] bias3 = calibrator.getInitialBias();
32330         final double[] bias4 = new double[3];
32331         calibrator.getInitialBias(bias4);
32332 
32333         assertArrayEquals(bias2, bias3, 0.0);
32334         assertArrayEquals(bias2, bias4, 0.0);
32335 
32336         // Force IllegalArgumentException
32337         try {
32338             calibrator.getInitialBias(new double[1]);
32339             fail("IllegalArgumentException expected but not thrown");
32340         } catch (final IllegalArgumentException ignore) {
32341         }
32342         try {
32343             calibrator.setInitialBias(new double[1]);
32344             fail("IllegalArgumentException expected but not thrown");
32345         } catch (final IllegalArgumentException ignore) {
32346         }
32347     }
32348 
32349     @Test
32350     public void testGetSetInitialBiasAsMatrix() throws WrongSizeException,
32351             LockedException {
32352 
32353         final KnownGravityNormAccelerometerCalibrator calibrator =
32354                 new KnownGravityNormAccelerometerCalibrator();
32355 
32356         // check default value
32357         final Matrix bias1 = calibrator.getInitialBiasAsMatrix();
32358         assertEquals(bias1, new Matrix(3, 1));
32359 
32360         // set new value
32361         final Matrix bias2 = generateBa();
32362         calibrator.setInitialBias(bias2);
32363 
32364         // check
32365         final Matrix bias3 = calibrator.getInitialBiasAsMatrix();
32366         final Matrix bias4 = new Matrix(3, 1);
32367         calibrator.getInitialBiasAsMatrix(bias4);
32368 
32369         assertEquals(bias2, bias3);
32370         assertEquals(bias2, bias4);
32371 
32372         // Force IllegalArgumentException
32373         try {
32374             calibrator.getInitialBiasAsMatrix(new Matrix(1, 1));
32375             fail("IllegalArgumentException expected but not thrown");
32376         } catch (final IllegalArgumentException ignore) {
32377         }
32378         try {
32379             calibrator.getInitialBiasAsMatrix(new Matrix(3, 3));
32380             fail("IllegalArgumentException expected but not thrown");
32381         } catch (final IllegalArgumentException ignore) {
32382         }
32383         try {
32384             calibrator.setInitialBias(new Matrix(1, 1));
32385             fail("IllegalArgumentException expected but not thrown");
32386         } catch (final IllegalArgumentException ignore) {
32387         }
32388         try {
32389             calibrator.setInitialBias(new Matrix(3, 3));
32390             fail("IllegalArgumentException expected but not thrown");
32391         } catch (final IllegalArgumentException ignore) {
32392         }
32393     }
32394 
32395     @Test
32396     public void testGetSetInitialMa() throws WrongSizeException, LockedException {
32397         final KnownGravityNormAccelerometerCalibrator calibrator =
32398                 new KnownGravityNormAccelerometerCalibrator();
32399 
32400         // test default value
32401         final Matrix ma1 = calibrator.getInitialMa();
32402         assertEquals(ma1, new Matrix(3, 3));
32403 
32404         // set new value
32405         final Matrix ma2 = generateMaGeneral();
32406         calibrator.setInitialMa(ma2);
32407 
32408         // check
32409         final Matrix ma3 = calibrator.getInitialMa();
32410         final Matrix ma4 = new Matrix(3, 3);
32411         calibrator.getInitialMa(ma4);
32412 
32413         assertEquals(ma2, ma3);
32414         assertEquals(ma2, ma4);
32415 
32416         // Force IllegalArgumentException
32417         try {
32418             calibrator.getInitialMa(new Matrix(1, 3));
32419             fail("IllegalArgumentException expected but not thrown");
32420         } catch (final IllegalArgumentException ignore) {
32421         }
32422         try {
32423             calibrator.getInitialMa(new Matrix(3, 1));
32424             fail("IllegalArgumentException expected but not thrown");
32425         } catch (final IllegalArgumentException ignore) {
32426         }
32427         try {
32428             calibrator.setInitialMa(new Matrix(1, 3));
32429             fail("IllegalArgumentException expected but not thrown");
32430         } catch (final IllegalArgumentException ignore) {
32431         }
32432         try {
32433             calibrator.setInitialMa(new Matrix(3, 1));
32434             fail("IllegalArgumentException expected but not thrown");
32435         } catch (final IllegalArgumentException ignore) {
32436         }
32437     }
32438 
32439     @Test
32440     public void testGetSetMeasurements() throws LockedException {
32441         final KnownGravityNormAccelerometerCalibrator calibrator =
32442                 new KnownGravityNormAccelerometerCalibrator();
32443 
32444         // check default value
32445         assertNull(calibrator.getMeasurements());
32446 
32447         // set new value
32448         final Collection<StandardDeviationBodyKinematics> measurements =
32449                 Collections.emptyList();
32450         calibrator.setMeasurements(measurements);
32451 
32452         // check
32453         assertSame(calibrator.getMeasurements(), measurements);
32454     }
32455 
32456     @Test
32457     public void testIsSetCommonAxisUsed() throws LockedException {
32458         final KnownGravityNormAccelerometerCalibrator calibrator =
32459                 new KnownGravityNormAccelerometerCalibrator();
32460 
32461         // check default value
32462         assertFalse(calibrator.isCommonAxisUsed());
32463 
32464         // set new value
32465         calibrator.setCommonAxisUsed(true);
32466 
32467         // check
32468         assertTrue(calibrator.isCommonAxisUsed());
32469     }
32470 
32471     @Test
32472     public void testGetSetListener() throws LockedException {
32473         final KnownGravityNormAccelerometerCalibrator calibrator =
32474                 new KnownGravityNormAccelerometerCalibrator();
32475 
32476         // check default value
32477         assertNull(calibrator.getListener());
32478 
32479         // set new value
32480         calibrator.setListener(this);
32481 
32482         // check
32483         assertSame(calibrator.getListener(), this);
32484     }
32485 
32486     @Test
32487     public void testGetSetGroundTruthGravityNorm1() throws LockedException {
32488         final KnownGravityNormAccelerometerCalibrator calibrator =
32489                 new KnownGravityNormAccelerometerCalibrator();
32490 
32491         // check default value
32492         assertNull(calibrator.getGroundTruthGravityNorm());
32493 
32494         // set new value
32495         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
32496         final double latitude = Math.toRadians(
32497                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
32498         final double longitude = Math.toRadians(
32499                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
32500         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
32501         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
32502         final NEDVelocity nedVelocity = new NEDVelocity();
32503         final ECEFPosition ecefPosition = new ECEFPosition();
32504         final ECEFVelocity ecefVelocity = new ECEFVelocity();
32505         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
32506                 ecefPosition, ecefVelocity);
32507         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
32508                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
32509         final double gravityNorm = gravity.getNorm();
32510 
32511         calibrator.setGroundTruthGravityNorm(gravityNorm);
32512 
32513         // check
32514         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(),
32515                 0.0);
32516         assertEquals(gravity.getNormAsAcceleration(),
32517                 calibrator.getGroundTruthGravityNormAsAcceleration());
32518         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
32519         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
32520         assertEquals(gravity.getNormAsAcceleration(), g);
32521     }
32522 
32523     @Test
32524     public void testGetSetGroundTruthGravityNorm2() throws LockedException {
32525         final KnownGravityNormAccelerometerCalibrator calibrator =
32526                 new KnownGravityNormAccelerometerCalibrator();
32527 
32528         // check default value
32529         assertNull(calibrator.getGroundTruthGravityNorm());
32530 
32531         // set new value
32532         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
32533         final double latitude = Math.toRadians(
32534                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
32535         final double longitude = Math.toRadians(
32536                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
32537         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
32538         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
32539         final NEDVelocity nedVelocity = new NEDVelocity();
32540         final ECEFPosition ecefPosition = new ECEFPosition();
32541         final ECEFVelocity ecefVelocity = new ECEFVelocity();
32542         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
32543                 ecefPosition, ecefVelocity);
32544         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
32545                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
32546         final Acceleration gravityNorm = gravity.getNormAsAcceleration();
32547 
32548         calibrator.setGroundTruthGravityNorm(gravityNorm);
32549 
32550         // check
32551         assertEquals(gravity.getNorm(),
32552                 calibrator.getGroundTruthGravityNorm(), 0.0);
32553         assertEquals(gravityNorm,
32554                 calibrator.getGroundTruthGravityNormAsAcceleration());
32555         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
32556         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
32557         assertEquals(gravityNorm, g);
32558     }
32559 
32560     @Test
32561     public void testIsReady() throws LockedException {
32562         final KnownGravityNormAccelerometerCalibrator calibrator =
32563                 new KnownGravityNormAccelerometerCalibrator();
32564 
32565         // check
32566         assertFalse(calibrator.isReady());
32567 
32568 
32569         // set empty measurements
32570         final Collection<StandardDeviationBodyKinematics> measurements1 =
32571                 Collections.emptyList();
32572         calibrator.setMeasurements(measurements1);
32573 
32574         // check
32575         assertFalse(calibrator.isReady());
32576 
32577 
32578         // set enough measurements for general case
32579         calibrator.setCommonAxisUsed(false);
32580 
32581         final List<StandardDeviationBodyKinematics> measurements2 = new ArrayList<>();
32582         for (int i = 0; i < KnownGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
32583             measurements2.add(new StandardDeviationBodyKinematics());
32584         }
32585         calibrator.setMeasurements(measurements2);
32586 
32587         // check
32588         assertFalse(calibrator.isReady());
32589 
32590 
32591         // set gravity norm
32592         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
32593         final double latitude = Math.toRadians(
32594                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
32595         final double longitude = Math.toRadians(
32596                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
32597         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
32598         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
32599         final NEDVelocity nedVelocity = new NEDVelocity();
32600         final ECEFPosition ecefPosition = new ECEFPosition();
32601         final ECEFVelocity ecefVelocity = new ECEFVelocity();
32602         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
32603                 ecefPosition, ecefVelocity);
32604         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
32605                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
32606 
32607         calibrator.setGroundTruthGravityNorm(gravity.getNorm());
32608 
32609         assertTrue(calibrator.isReady());
32610 
32611         // set enough measurements for common axis case
32612         measurements2.clear();
32613         for (int i = 0; i < KnownGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMON_Z_AXIS; i++) {
32614             measurements2.add(new StandardDeviationBodyKinematics());
32615         }
32616         calibrator.setMeasurements(measurements2);
32617 
32618         // check
32619         assertFalse(calibrator.isReady());
32620 
32621         calibrator.setCommonAxisUsed(true);
32622 
32623         assertTrue(calibrator.isReady());
32624     }
32625 
32626     @Test
32627     public void testCalibrateForGeneralCaseWithMinimumMeasuresAndNoNoise()
32628             throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
32629             LockedException, NotReadyException {
32630 
32631         int numValid = 0;
32632         for (int t = 0; t < TIMES; t++) {
32633             final Matrix ba = generateBa();
32634             final Matrix bg = generateBg();
32635             final Matrix ma = generateMaGeneral();
32636             final Matrix mg = generateMg();
32637             final Matrix gg = generateGg();
32638             // when using minimum number of measurements we must not add any noise so that
32639             // a solution is found, when adding more measurements, certain noise can be added
32640             final double accelNoiseRootPSD = 0.0;
32641             final double gyroNoiseRootPSD = 0.0;
32642             final double accelQuantLevel = 0.0;
32643             final double gyroQuantLevel = 0.0;
32644 
32645             final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
32646                     gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
32647 
32648             final Random random = new Random();
32649             final UniformRandomizer randomizer = new UniformRandomizer(random);
32650             final double latitude = Math.toRadians(
32651                     randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
32652             final double longitude = Math.toRadians(
32653                     randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
32654             final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
32655             final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
32656             final NEDVelocity nedVelocity = new NEDVelocity();
32657             final ECEFPosition ecefPosition = new ECEFPosition();
32658             final ECEFVelocity ecefVelocity = new ECEFVelocity();
32659             NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
32660                     ecefPosition, ecefVelocity);
32661             final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
32662                     ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
32663 
32664             final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
32665             final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
32666             final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
32667 
32668             final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
32669             for (int i = 0; i < KnownGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
32670                 final double roll = Math.toRadians(
32671                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32672                 final double pitch = Math.toRadians(
32673                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32674                 final double yaw = Math.toRadians(
32675                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32676                 final CoordinateTransformation nedC = new CoordinateTransformation(
32677                         roll, pitch, yaw, FrameType.BODY_FRAME,
32678                         FrameType.LOCAL_NAVIGATION_FRAME);
32679 
32680                 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
32681                 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
32682                         .convertNEDtoECEFAndReturnNew(nedFrame);
32683 
32684                 // compute ground-truth kinematics that should be generated at provided
32685                 // position, velocity and orientation
32686                 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
32687                         .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
32688                                 ecefFrame);
32689 
32690                 // apply known calibration parameters to distort ground-truth and generate a
32691                 // measured kinematics sample
32692                 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
32693                         .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
32694 
32695                 final StandardDeviationBodyKinematics measurement =
32696                         new StandardDeviationBodyKinematics(measuredKinematics,
32697                                 specificForceStandardDeviation,
32698                                 angularRateStandardDeviation);
32699                 measurements.add(measurement);
32700             }
32701 
32702             // When we have the minimum number of measurements, we need to provide
32703             // an initial solution close to the true solution
32704             final KnownGravityNormAccelerometerCalibrator calibrator =
32705                     new KnownGravityNormAccelerometerCalibrator(
32706                             gravity.getNorm(), measurements,
32707                             false, ba, ma, this);
32708 
32709             // estimate
32710             reset();
32711             assertTrue(calibrator.isReady());
32712             assertFalse(calibrator.isRunning());
32713             assertEquals(mCalibrateStart, 0);
32714             assertEquals(mCalibrateEnd, 0);
32715 
32716             try {
32717                 calibrator.calibrate();
32718             } catch (final CalibrationException e) {
32719                 continue;
32720             }
32721 
32722             // check
32723             assertTrue(calibrator.isReady());
32724             assertFalse(calibrator.isRunning());
32725             assertEquals(mCalibrateStart, 1);
32726             assertEquals(mCalibrateEnd, 1);
32727 
32728             final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
32729             final Matrix estimatedMa = calibrator.getEstimatedMa();
32730 
32731             if (!ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR)) {
32732                 continue;
32733             }
32734             if (!ma.equals(estimatedMa, ABSOLUTE_ERROR)) {
32735                 continue;
32736             }
32737 
32738             assertTrue(ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR));
32739             assertTrue(ma.equals(estimatedMa, ABSOLUTE_ERROR));
32740 
32741             numValid++;
32742 
32743             break;
32744         }
32745 
32746         assertTrue(numValid > 0);
32747     }
32748 
32749     @Test
32750     public void testCalibrateForGeneralCaseWithLargeNumberOfMeasurementsAndNoise()
32751             throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
32752             LockedException, NotReadyException {
32753 
32754         int numValid = 0;
32755         for (int t = 0; t < TIMES; t++) {
32756             final Matrix ba = generateBa();
32757             final Matrix bg = generateBg();
32758             final Matrix ma = generateMaGeneral();
32759             final Matrix mg = generateMg();
32760             final Matrix gg = generateGg();
32761             final double accelNoiseRootPSD = getAccelNoiseRootPSD();
32762             final double gyroNoiseRootPSD = getGyroNoiseRootPSD();
32763             final double accelQuantLevel = 0.0;
32764             final double gyroQuantLevel = 0.0;
32765 
32766             final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
32767                     gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
32768 
32769             final Random random = new Random();
32770             final UniformRandomizer randomizer = new UniformRandomizer(random);
32771             final double latitude = Math.toRadians(
32772                     randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
32773             final double longitude = Math.toRadians(
32774                     randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
32775             final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
32776             final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
32777             final NEDVelocity nedVelocity = new NEDVelocity();
32778             final ECEFPosition ecefPosition = new ECEFPosition();
32779             final ECEFVelocity ecefVelocity = new ECEFVelocity();
32780             NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
32781                     ecefPosition, ecefVelocity);
32782             final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
32783                     ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
32784 
32785             final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
32786             final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
32787             final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
32788 
32789             final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
32790             for (int i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
32791                 final double roll = Math.toRadians(
32792                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32793                 final double pitch = Math.toRadians(
32794                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32795                 final double yaw = Math.toRadians(
32796                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32797                 final CoordinateTransformation nedC = new CoordinateTransformation(
32798                         roll, pitch, yaw, FrameType.BODY_FRAME,
32799                         FrameType.LOCAL_NAVIGATION_FRAME);
32800 
32801                 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
32802                 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
32803                         .convertNEDtoECEFAndReturnNew(nedFrame);
32804 
32805                 // compute ground-truth kinematics that should be generated at provided
32806                 // position, velocity and orientation
32807                 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
32808                         .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
32809                                 ecefFrame);
32810 
32811                 // apply known calibration parameters to distort ground-truth and generate a
32812                 // measured kinematics sample
32813                 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
32814                         .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
32815 
32816                 final StandardDeviationBodyKinematics measurement =
32817                         new StandardDeviationBodyKinematics(measuredKinematics,
32818                                 specificForceStandardDeviation,
32819                                 angularRateStandardDeviation);
32820                 measurements.add(measurement);
32821             }
32822 
32823             // When we have a large number of measurements, we do not need to provide
32824             // an initial solution as it will probably converge to true true solution
32825             final KnownGravityNormAccelerometerCalibrator calibrator =
32826                     new KnownGravityNormAccelerometerCalibrator(
32827                             gravity.getNorm(),
32828                             measurements, false, this);
32829 
32830             // estimate
32831             reset();
32832             assertTrue(calibrator.isReady());
32833             assertFalse(calibrator.isRunning());
32834             assertEquals(mCalibrateStart, 0);
32835             assertEquals(mCalibrateEnd, 0);
32836 
32837             try {
32838                 calibrator.calibrate();
32839             } catch (final CalibrationException e) {
32840                 continue;
32841             }
32842 
32843             // check
32844             assertTrue(calibrator.isReady());
32845             assertFalse(calibrator.isRunning());
32846             assertEquals(mCalibrateStart, 1);
32847             assertEquals(mCalibrateEnd, 1);
32848 
32849             final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
32850             final Matrix estimatedMa = calibrator.getEstimatedMa();
32851 
32852             if (!ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR)) {
32853                 continue;
32854             }
32855             if (!ma.equals(estimatedMa, 6.0 * LARGE_ABSOLUTE_ERROR)) {
32856                 continue;
32857             }
32858 
32859             assertTrue(ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR));
32860             assertTrue(ma.equals(estimatedMa, 6.0 * LARGE_ABSOLUTE_ERROR));
32861 
32862             numValid++;
32863 
32864             break;
32865         }
32866 
32867         assertTrue(numValid > 0);
32868     }
32869 
32870     @Test
32871     public void testCalibrateForCommonAxisCaseWithMinimumMeasuresAndNoNoise()
32872             throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
32873             LockedException, NotReadyException {
32874 
32875         int numValid = 0;
32876         for (int t = 0; t < TIMES; t++) {
32877             final Matrix ba = generateBa();
32878             final Matrix bg = generateBg();
32879             final Matrix ma = generateMaCommonAxis();
32880             final Matrix mg = generateMg();
32881             final Matrix gg = generateGg();
32882             // when using minimum number of measurements we must not add any noise so that
32883             // a solution is found, when adding more measurements, certain noise can be added
32884             final double accelNoiseRootPSD = 0.0;
32885             final double gyroNoiseRootPSD = 0.0;
32886             final double accelQuantLevel = 0.0;
32887             final double gyroQuantLevel = 0.0;
32888 
32889             final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
32890                     gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
32891 
32892             final Random random = new Random();
32893             final UniformRandomizer randomizer = new UniformRandomizer(random);
32894             final double latitude = Math.toRadians(
32895                     randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
32896             final double longitude = Math.toRadians(
32897                     randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
32898             final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
32899             final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
32900             final NEDVelocity nedVelocity = new NEDVelocity();
32901             final ECEFPosition ecefPosition = new ECEFPosition();
32902             final ECEFVelocity ecefVelocity = new ECEFVelocity();
32903             NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
32904                     ecefPosition, ecefVelocity);
32905             final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
32906                     ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
32907 
32908             final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
32909             final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
32910             final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
32911 
32912             final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
32913             for (int i = 0; i < KnownGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMON_Z_AXIS; i++) {
32914                 final double roll = Math.toRadians(
32915                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32916                 final double pitch = Math.toRadians(
32917                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32918                 final double yaw = Math.toRadians(
32919                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32920                 final CoordinateTransformation nedC = new CoordinateTransformation(
32921                         roll, pitch, yaw, FrameType.BODY_FRAME,
32922                         FrameType.LOCAL_NAVIGATION_FRAME);
32923 
32924                 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
32925                 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
32926                         .convertNEDtoECEFAndReturnNew(nedFrame);
32927 
32928                 // compute ground-truth kinematics that should be generated at provided
32929                 // position, velocity and orientation
32930                 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
32931                         .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
32932                                 ecefFrame);
32933 
32934                 // apply known calibration parameters to distort ground-truth and generate a
32935                 // measured kinematics sample
32936                 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
32937                         .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
32938 
32939                 final StandardDeviationBodyKinematics measurement =
32940                         new StandardDeviationBodyKinematics(measuredKinematics,
32941                                 specificForceStandardDeviation,
32942                                 angularRateStandardDeviation);
32943                 measurements.add(measurement);
32944             }
32945 
32946             // When we have the minimum number of measurements, we need to provide
32947             // an initial solution close to the true solution
32948             final KnownGravityNormAccelerometerCalibrator calibrator =
32949                     new KnownGravityNormAccelerometerCalibrator(
32950                             gravity.getNorm(), measurements,
32951                             true, ba, ma, this);
32952 
32953             // estimate
32954             reset();
32955             assertTrue(calibrator.isReady());
32956             assertFalse(calibrator.isRunning());
32957             assertEquals(mCalibrateStart, 0);
32958             assertEquals(mCalibrateEnd, 0);
32959 
32960             try {
32961                 calibrator.calibrate();
32962             } catch (final CalibrationException e) {
32963                 continue;
32964             }
32965 
32966             // check
32967             assertTrue(calibrator.isReady());
32968             assertFalse(calibrator.isRunning());
32969             assertEquals(mCalibrateStart, 1);
32970             assertEquals(mCalibrateEnd, 1);
32971 
32972             final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
32973             final Matrix estimatedMa = calibrator.getEstimatedMa();
32974 
32975             if (!ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR)) {
32976                 continue;
32977             }
32978             if (!ma.equals(estimatedMa, ABSOLUTE_ERROR)) {
32979                 continue;
32980             }
32981 
32982             assertTrue(ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR));
32983             assertTrue(ma.equals(estimatedMa, ABSOLUTE_ERROR));
32984 
32985             numValid++;
32986 
32987             break;
32988         }
32989 
32990         assertTrue(numValid > 0);
32991     }
32992 
32993     @Test
32994     public void testCalibrateForCommonAxisCaseWithLargeNumberOfMeasurementsAndNoise()
32995             throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
32996             LockedException, NotReadyException {
32997 
32998         int numValid = 0;
32999         for (int t = 0; t < TIMES; t++) {
33000             final Matrix ba = generateBa();
33001             final Matrix bg = generateBg();
33002             final Matrix ma = generateMaCommonAxis();
33003             final Matrix mg = generateMg();
33004             final Matrix gg = generateGg();
33005             final double accelNoiseRootPSD = getAccelNoiseRootPSD();
33006             final double gyroNoiseRootPSD = getGyroNoiseRootPSD();
33007             final double accelQuantLevel = 0.0;
33008             final double gyroQuantLevel = 0.0;
33009 
33010             final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
33011                     gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
33012 
33013             final Random random = new Random();
33014             final UniformRandomizer randomizer = new UniformRandomizer(random);
33015             final double latitude = Math.toRadians(
33016                     randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
33017             final double longitude = Math.toRadians(
33018                     randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
33019             final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
33020             final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
33021             final NEDVelocity nedVelocity = new NEDVelocity();
33022             final ECEFPosition ecefPosition = new ECEFPosition();
33023             final ECEFVelocity ecefVelocity = new ECEFVelocity();
33024             NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
33025                     ecefPosition, ecefVelocity);
33026             final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
33027                     ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
33028 
33029             final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
33030             final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
33031             final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
33032 
33033             final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
33034             for (int i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
33035                 final double roll = Math.toRadians(
33036                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
33037                 final double pitch = Math.toRadians(
33038                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
33039                 final double yaw = Math.toRadians(
33040                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
33041                 final CoordinateTransformation nedC = new CoordinateTransformation(
33042                         roll, pitch, yaw, FrameType.BODY_FRAME,
33043                         FrameType.LOCAL_NAVIGATION_FRAME);
33044 
33045                 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
33046                 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
33047                         .convertNEDtoECEFAndReturnNew(nedFrame);
33048 
33049                 // compute ground-truth kinematics that should be generated at provided
33050                 // position, velocity and orientation
33051                 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
33052                         .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
33053                                 ecefFrame);
33054 
33055                 // apply known calibration parameters to distort ground-truth and generate a
33056                 // measured kinematics sample
33057                 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
33058                         .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
33059 
33060                 final StandardDeviationBodyKinematics measurement =
33061                         new StandardDeviationBodyKinematics(measuredKinematics,
33062                                 specificForceStandardDeviation,
33063                                 angularRateStandardDeviation);
33064                 measurements.add(measurement);
33065             }
33066 
33067             // When we have a large number of measurements, we do not need to provide
33068             // an initial solution as it will probably converge to true true solution
33069             final KnownGravityNormAccelerometerCalibrator calibrator =
33070                     new KnownGravityNormAccelerometerCalibrator(
33071                             gravity.getNorm(), measurements,
33072                             true, this);
33073 
33074             // estimate
33075             reset();
33076             assertTrue(calibrator.isReady());
33077             assertFalse(calibrator.isRunning());
33078             assertEquals(mCalibrateStart, 0);
33079             assertEquals(mCalibrateEnd, 0);
33080 
33081             try {
33082                 calibrator.calibrate();
33083             } catch (final CalibrationException e) {
33084                 continue;
33085             }
33086 
33087             // check
33088             assertTrue(calibrator.isReady());
33089             assertFalse(calibrator.isRunning());
33090             assertEquals(mCalibrateStart, 1);
33091             assertEquals(mCalibrateEnd, 1);
33092 
33093             final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
33094             final Matrix estimatedMa = calibrator.getEstimatedMa();
33095 
33096             if (!ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR)) {
33097                 continue;
33098             }
33099             if (!ma.equals(estimatedMa, LARGE_ABSOLUTE_ERROR)) {
33100                 continue;
33101             }
33102 
33103             assertTrue(ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR));
33104             assertTrue(ma.equals(estimatedMa, LARGE_ABSOLUTE_ERROR));
33105 
33106             numValid++;
33107 
33108             break;
33109         }
33110 
33111         assertTrue(numValid > 0);
33112     }
33113     
33114     @Override
33115     public void onCalibrateStart(final KnownGravityNormAccelerometerCalibrator calibrator) {
33116         checkLocked(calibrator);
33117         mCalibrateStart++;
33118     }
33119 
33120     @Override
33121     public void onCalibrateEnd(final KnownGravityNormAccelerometerCalibrator calibrator) {
33122         checkLocked(calibrator);
33123         mCalibrateEnd++;
33124     }
33125 
33126     private void reset() {
33127         mCalibrateStart = 0;
33128         mCalibrateEnd = 0;
33129     }
33130 
33131     private void checkLocked(final KnownGravityNormAccelerometerCalibrator calibrator) {
33132         assertTrue(calibrator.isRunning());
33133         try {
33134             calibrator.setInitialBiasX(0.0);
33135             fail("LockedException expected but not thrown");
33136         } catch (final LockedException ignore) {
33137         }
33138         try {
33139             calibrator.setInitialBiasY(0.0);
33140             fail("LockedException expected but not thrown");
33141         } catch (final LockedException ignore) {
33142         }
33143         try {
33144             calibrator.setInitialBiasZ(0.0);
33145             fail("LockedException expected but not thrown");
33146         } catch (final LockedException ignore) {
33147         }
33148         try {
33149             calibrator.setInitialBiasX(null);
33150             fail("LockedException expected but not thrown");
33151         } catch (final LockedException ignore) {
33152         }
33153         try {
33154             calibrator.setInitialBiasY(null);
33155             fail("LockedException expected but not thrown");
33156         } catch (final LockedException ignore) {
33157         }
33158         try {
33159             calibrator.setInitialBiasZ(null);
33160             fail("LockedException expected but not thrown");
33161         } catch (final LockedException ignore) {
33162         }
33163         try {
33164             calibrator.setInitialBias(0.0, 0.0, 0.0);
33165             fail("LockedException expected but not thrown");
33166         } catch (final LockedException ignore) {
33167         }
33168         try {
33169             calibrator.setInitialBias(null, null, null);
33170             fail("LockedException expected but not thrown");
33171         } catch (final LockedException ignore) {
33172         }
33173         try {
33174             calibrator.setInitialSx(0.0);
33175             fail("LockedException expected but not thrown");
33176         } catch (final LockedException ignore) {
33177         }
33178         try {
33179             calibrator.setInitialSy(0.0);
33180             fail("LockedException expected but not thrown");
33181         } catch (final LockedException ignore) {
33182         }
33183         try {
33184             calibrator.setInitialSz(0.0);
33185             fail("LockedException expected but not thrown");
33186         } catch (final LockedException ignore) {
33187         }
33188         try {
33189             calibrator.setInitialMxy(0.0);
33190             fail("LockedException expected but not thrown");
33191         } catch (final LockedException ignore) {
33192         }
33193         try {
33194             calibrator.setInitialMxz(0.0);
33195             fail("LockedException expected but not thrown");
33196         } catch (final LockedException ignore) {
33197         }
33198         try {
33199             calibrator.setInitialMyx(0.0);
33200             fail("LockedException expected but not thrown");
33201         } catch (final LockedException ignore) {
33202         }
33203         try {
33204             calibrator.setInitialMyz(0.0);
33205             fail("LockedException expected but not thrown");
33206         } catch (final LockedException ignore) {
33207         }
33208         try {
33209             calibrator.setInitialMzx(0.0);
33210             fail("LockedException expected but not thrown");
33211         } catch (final LockedException ignore) {
33212         }
33213         try {
33214             calibrator.setInitialMzy(0.0);
33215             fail("LockedException expected but not thrown");
33216         } catch (final LockedException ignore) {
33217         }
33218         try {
33219             calibrator.setInitialScalingFactors(0.0, 0.0, 0.0);
33220             fail("LockedException expected but not thrown");
33221         } catch (final LockedException ignore) {
33222         }
33223         try {
33224             calibrator.setInitialCrossCouplingErrors(
33225                     0.0, 0.0, 0.0,
33226                     0.0, 0.0, 0.0);
33227             fail("LockedException expected but not thrown");
33228         } catch (final LockedException ignore) {
33229         }
33230         try {
33231             calibrator.setInitialScalingFactorsAndCrossCouplingErrors(
33232                     0.0, 0.0, 0.0,
33233                     0.0, 0.0, 0.0,
33234                     0.0, 0.0, 0.0);
33235             fail("LockedException expected but not thrown");
33236         } catch (final LockedException ignore) {
33237         }
33238         try {
33239             calibrator.setInitialBias((double[]) null);
33240             fail("LockedException expected but not thrown");
33241         } catch (final LockedException ignore) {
33242         }
33243         try {
33244             calibrator.setInitialBias((Matrix) null);
33245             fail("LockedException expected but not thrown");
33246         } catch (final LockedException ignore) {
33247         }
33248         try {
33249             calibrator.setInitialMa(null);
33250             fail("LockedException expected but not thrown");
33251         } catch (final LockedException ignore) {
33252         }
33253         try {
33254             calibrator.setGroundTruthGravityNorm(0.0);
33255             fail("LockedException expected but not thrown");
33256         } catch (final LockedException ignore) {
33257         }
33258         try {
33259             calibrator.setGroundTruthGravityNorm((Acceleration) null);
33260             fail("LockedException expected but not thrown");
33261         } catch (final LockedException ignore) {
33262         }
33263         try {
33264             calibrator.setMeasurements(null);
33265             fail("LockedException expected but not thrown");
33266         } catch (final LockedException ignore) {
33267         }
33268         try {
33269             calibrator.setCommonAxisUsed(true);
33270             fail("LockedException expected but not thrown");
33271         } catch (final LockedException ignore) {
33272         }
33273         try {
33274             calibrator.setListener(this);
33275             fail("LockedException expected but not thrown");
33276         } catch (final LockedException ignore) {
33277         }
33278         try {
33279             calibrator.calibrate();
33280             fail("LockedException expected but not thrown");
33281         } catch (final LockedException ignore) {
33282         } catch (final Exception e) {
33283             fail("LockedException expected but not thrown");
33284         }
33285     }
33286 
33287     private Matrix generateBa() {
33288         return Matrix.newFromArray(new double[]{
33289                 900 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
33290                 -1300 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
33291                 800 * MICRO_G_TO_METERS_PER_SECOND_SQUARED});
33292     }
33293 
33294     private Matrix generateBg() {
33295         return Matrix.newFromArray(new double[]{
33296                 -9 * DEG_TO_RAD / 3600.0,
33297                 13 * DEG_TO_RAD / 3600.0,
33298                 -8 * DEG_TO_RAD / 3600.0});
33299     }
33300 
33301     private Matrix generateMaGeneral() throws WrongSizeException {
33302         final Matrix result = new Matrix(3, 3);
33303         result.fromArray(new double[]{
33304                 500e-6, -300e-6, 200e-6,
33305                 -150e-6, -600e-6, 250e-6,
33306                 -250e-6, 100e-6, 450e-6
33307         }, false);
33308 
33309         return result;
33310     }
33311 
33312     private Matrix generateMaCommonAxis() throws WrongSizeException {
33313         final Matrix result = new Matrix(3, 3);
33314         result.fromArray(new double[]{
33315                 500e-6, -300e-6, 200e-6,
33316                 0.0, -600e-6, 250e-6,
33317                 0.0, 0.0, 450e-6
33318         }, false);
33319 
33320         return result;
33321     }
33322 
33323     private Matrix generateMg() throws WrongSizeException {
33324         final Matrix result = new Matrix(3, 3);
33325         result.fromArray(new double[]{
33326                 400e-6, -300e-6, 250e-6,
33327                 0.0, -300e-6, -150e-6,
33328                 0.0, 0.0, -350e-6
33329         }, false);
33330 
33331         return result;
33332     }
33333 
33334     private Matrix generateGg() throws WrongSizeException {
33335         final Matrix result = new Matrix(3, 3);
33336         final double tmp = DEG_TO_RAD / (3600 * 9.80665);
33337         result.fromArray(new double[]{
33338                 0.9 * tmp, -1.1 * tmp, -0.6 * tmp,
33339                 -0.5 * tmp, 1.9 * tmp, -1.6 * tmp,
33340                 0.3 * tmp, 1.1 * tmp, -1.3 * tmp
33341         }, false);
33342 
33343         return result;
33344     }
33345 
33346     private double getAccelNoiseRootPSD() {
33347         return 100.0 * MICRO_G_TO_METERS_PER_SECOND_SQUARED;
33348     }
33349 
33350     private double getGyroNoiseRootPSD() {
33351         return 0.01 * DEG_TO_RAD / 60.0;
33352     }
33353 }